diff --git a/.gitignore b/.gitignore index e8257e5..6eacefb 100644 --- a/.gitignore +++ b/.gitignore @@ -11,4 +11,7 @@ venv/ env/ venv.bak/ env.bak/ -logs/ \ No newline at end of file +logs/ + +*.zip +tuned_teleop_configs.yaml \ No newline at end of file diff --git a/README.md b/README.md index 805298f..ea6ead0 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,4 @@ + # AgileX Piper Robot Teleoperation with Neuracore
@@ -12,286 +13,272 @@
-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 -- 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 +# 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 ] -``` +cd examples +python 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 ] -``` +cd examples +python 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). +* **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).* -**Controls**: -- Same as script 1, plus: -- **Right Joystick Press**: Start/stop data recording +### 3. Replay Neuracore Episodes (Hardware Validation) -### 3. Replay Neuracore Episodes +**Script:** `examples/3_replay_neuracore_episodes.py` -**Script**: `examples/3_replay_neuracore_episodes.py` +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 ] -``` +cd examples +python 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 ] -``` +# Load from a local model file +cd examples +python 4_rollout_neuracore_policy.py --model-path -or +# Load from a cloud training run +python 4_rollout_neuracore_policy.py --train-run-name -```bash -python examples/4_rollout_neuracore_policy.py --model-path [--ip-address ] -``` - -**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 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 -``` +cd examples +python 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 from Dataset +### 6. Visualize Policy Offline -**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 +cd examples +python 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. diff --git a/example-agilex-dictionary.txt b/example-agilex-dictionary.txt index 5a0a7f0..57315b0 100644 --- a/example-agilex-dictionary.txt +++ b/example-agilex-dictionary.txt @@ -56,4 +56,12 @@ evdev keystate pynput ungrab +tele +filt +targs +infile +aioice +aiortc +halid +pruthvi diff --git a/examples/1_tune_teleop_params.py b/examples/1_tune_teleop_params.py index 3a8ea8e..1b3ef40 100644 --- a/examples/1_tune_teleop_params.py +++ b/examples/1_tune_teleop_params.py @@ -1,14 +1,14 @@ #!/usr/bin/env python3 -"""Piper Robot Teleoperation with Meta Quest Controller - REAL ROBOT CONTROL. +"""Piper Robot Teleoperation Tuning - 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 +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 os import sys import threading import time @@ -16,462 +16,345 @@ from pathlib import Path import numpy as np +import yaml # type: ignore[import] -# 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.config_parser import load_ik_config + +# Only import the fixed hardware constants that are left in configs.py 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.shared_actions import move_robot_home, toggle_robot_enabled +from common.system_bootstrap import bootstrap_robot_system from common.threads.quest_reader import quest_reader_thread -from common.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") +# --------------------------------------------------------------------------- +# Dynamic Action Callbacks +# --------------------------------------------------------------------------- -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. +def _step_wrist_joint( + data_manager: DataManager, + robot_controller: PiperController, + visualizer: RobotVisualizer, + direction: int, +) -> None: + """Apply a relative manual step to the wrist joint using the dynamically tuned step degree.""" data_manager.set_teleop_state(False, None, None) 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) + 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 -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) + robot_controller.set_target_joint_angles(target_joint_angles) + data_manager.set_target_joint_angles(target_joint_angles) -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: DataManager, visualizer: RobotVisualizer) -> None: + """Toggle the movement scaling mode between standard and dynamically-tuned precision.""" + enabled = data_manager.toggle_slow_scaling_mode_enabled() + if enabled: print( - "๐Ÿข Button LJ pressed - Slow scaling enabled " - f"(translation={SLOW_TRANSLATION_SCALE:.3f}, " - f"rotation={SLOW_ROTATION_SCALE:.3f})" + f"๐Ÿข Slow scaling enabled (trans={visualizer.get_slow_translation_scale():.3f}, rot={visualizer.get_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})" + f"๐Ÿ‡ Slow scaling disabled (trans={visualizer.get_translation_scale():.3f}, rot={visualizer.get_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), -) +def save_config_to_yaml( + visualizer: RobotVisualizer, filepath: str = "ik_conf/tuned_teleop_configs.yaml" +) -> None: + """Extracts all current UI slider values and saves them to a YAML file.""" + try: + # Ensure the directory exists before saving + os.makedirs(os.path.dirname(filepath), exist_ok=True) -# 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() + 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)}" ) - 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 + 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 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 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 & Handle ADB Exceptions + print("\n๐ŸŽฎ Initializing Meta Quest reader...") + try: + quest_reader = MetaQuestReader( + ip_address=args.ip_address, + port=5555, + run=True, + axis_mask=META_QUEST_AXIS_MASK, + ) + except (Exception, SystemExit): + 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 Web UI + 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() + + # 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( + 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, 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. Open http://localhost:8080 to tune. Press Ctrl+C to exit.\n" + ) + + # 6. Main Visualization Loop + dt = 1.0 / VISUALIZATION_RATE + try: + while True: + iteration_start = time.time() + + # Poll GUI for real-time scale tuning updates + t_scale = ( + visualizer.get_slow_translation_scale() + if data_manager.get_slow_scaling_mode_enabled() + else visualizer.get_translation_scale() ) - else: - visualizer.update_controller_status_display(None, connected=False) - - # Update teleop status display - visualizer.update_teleop_status(teleop_active) + 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) - # Update RGB camera image in Viser GUI (if available) - if rgb_image is not None: + # 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] + if controller_transform is not None + else None + ), + connected=controller_transform is not None, + ) + visualizer.update_teleop_status(teleop_active) + visualizer.update_target_visualization(data_manager.get_target_pose()) visualizer.update_rgb_image(rgb_image) - # 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), - ) + # 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) + ) - # 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.") + # Maintain UI refresh 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(timeout=2.0) + robot_controller.cleanup() + visualizer.stop() + print("๐Ÿ‘‹ Demo stopped.") diff --git a/examples/2_collect_teleop_data_with_neuracore.py b/examples/2_collect_teleop_data_with_neuracore.py index 2d04bd5..0d3d697 100644 --- a/examples/2_collect_teleop_data_with_neuracore.py +++ b/examples/2_collect_teleop_data_with_neuracore.py @@ -1,408 +1,253 @@ #!/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 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 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 +# --------------------------------------------------------------------------- +# 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 ( - 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.config_parser import load_ik_config +from common.configs import META_QUEST_AXIS_MASK, URDF_PATH 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.shared_actions import ( + move_robot_home, + neuracore_logging_callback, + toggle_recording, + toggle_robot_enabled, +) +from common.system_bootstrap import bootstrap_robot_system from common.threads.quest_reader import quest_reader_thread -from 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 +# --------------------------------------------------------------------------- +# Helper Functions +# --------------------------------------------------------------------------- -def log_to_neuracore_on_change_callback( - name: str, payload: dict[str, Any], timestamp: float + +def _step_wrist_joint( + data_manager: DataManager, robot_controller: PiperController, delta_degrees: float ) -> None: - """Log data to queue on change callback.""" - # Call appropriate Neuracore logging function - 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)) - 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)") - 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)") - 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. + """Apply a relative step adjustment to the robot's wrist joint target angle.""" data_manager.set_teleop_state(False, None, None) robot_controller.set_control_mode(PiperController.ControlMode.JOINT_SPACE) 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 + # 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 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})" +def toggle_slow_scaling(data_manager: DataManager, tele_p: dict) -> None: + """Toggle the teleoperation sensitivity between standard and slow (precision) scaling.""" + enabled = data_manager.toggle_slow_scaling_mode_enabled() + if enabled: + data_manager.set_teleop_scaling( + 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( - "๐Ÿ‡ Button LJ pressed - Slow scaling disabled " - f"(translation={TRANSLATION_SCALE:.3f}, " - f"rotation={ROTATION_SCALE:.3f})" + 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="Piper Robot Teleoperation with Neuracore Data Collection - REAL ROBOT CONTROL" + description="Teleop with Neuracore Data Collection" ) parser.add_argument( "--ip-address", type=str, default=None, - help="IP address of Meta Quest device (optional, defaults to None for auto-discovery)", + help="IP address of the Meta Quest headset.", ) parser.add_argument( "--dataset-name", type=str, default=None, - help="Name for the dataset (optional, defaults to auto-generated timestamp-based name)", + 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) - 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("=" * 60 + "\nPIPER TELEOP DATA COLLECTION\n" + "=" * 60) + + # Load the YAML configuration dictionary + config = load_ik_config(args.ik_config) + tele_p = config.get("teleop_parameters", {}) + 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, + robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False ) - # Create dataset + # 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')}" ) - print(f"\n๐Ÿ”ง Creating dataset {dataset_name}...") nc.create_dataset( - name=dataset_name, - description="Teleop data collection for Piper robot", + 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, + # --------------------------------------------------------- + # 2. Hardware & Subsystem Bootstrapping + # --------------------------------------------------------- + data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( + config, start_ik=True, start_camera=True ) - # 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() + # Bind the unified Neuracore logger so state changes automatically push to the cloud + data_manager.set_on_change_callback(neuracore_logging_callback) - # Initialize Meta Quest reader + # --------------------------------------------------------- + # 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): + 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") - # 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) + 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_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) + ) - # 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() + 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 + print( + "\n๐Ÿš€ Ready! Use Meta Quest controllers. Press Right Joystick to Record. Press Ctrl+C to exit.\n" ) - 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() + # --------------------------------------------------------- + # 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. Exception: {e}") - print("Traceback:") - traceback.print_exc() + print(f"\nโŒ Unhandled 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. Graceful Teardown & Cleanup + # --------------------------------------------------------- + print("\n๐Ÿงน Cleaning up subsystems...") + 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(timeout=2.0) + nc.logout() robot_controller.cleanup() - - print("\n๐Ÿ‘‹ Demo stopped.") + print("๐Ÿ‘‹ Demo stopped.") diff --git a/examples/3_replay_neuracore_episodes.py b/examples/3_replay_neuracore_episodes.py index 2153bcb..d52150c 100644 --- a/examples/3_replay_neuracore_episodes.py +++ b/examples/3_replay_neuracore_episodes.py @@ -1,4 +1,36 @@ -"""Replay a recorded Neuracore dataset on the Piper robot.""" +#!/usr/bin/env python3 +"""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 @@ -9,37 +41,82 @@ 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 +# --------------------------------------------------------------------------- +# 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 +from common.dataset_helpers import load_and_sync_dataset +from neuracore_types import DataType, SynchronizedPoint + 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) +# --------------------------------------------------------------------------- +# Helper Functions +# --------------------------------------------------------------------------- +def wait_for_home(robot_controller: PiperController, timeout: int = 10) -> bool: + """Command the robot to its home position and block until it arrives.""" + robot_controller.move_to_home() + for remaining in range(timeout, 0, -1): + if robot_controller.is_robot_homed(): + 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("\nโŒ Error: Robot did not reach home position within the timeout.") + return False + + +# --------------------------------------------------------------------------- +# Main Execution +# --------------------------------------------------------------------------- +if __name__ == "__main__": + # --------------------------------------------------------- + # 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() - # Initialize robot controller + 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", @@ -48,164 +125,149 @@ def main() -> None: 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 + # --------------------------------------------------------- + # 3. Neuracore Connection & Dataset Synchronization + # --------------------------------------------------------- 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] = [ + input_mods = [ DataType.JOINT_POSITIONS, DataType.RGB_IMAGES, DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS, ] - output_modalities: list[DataType] = [ + output_mods = [ 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, + synced_dataset = load_and_sync_dataset( + args.dataset_name, args.frequency, input_mods, output_mods ) - # Determine which episodes to play - episode_indices: list[int] = [] + 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 episodes.") + 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.") - # Play episodes + # --------------------------------------------------------- + # 4. Playback Loop + # --------------------------------------------------------- try: for episode_idx in episode_indices: + episode = synced_dataset[episode_idx] - 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}") + if len(episode) == 0: + print(f"โš ๏ธ Warning: Episode {episode_idx} is empty. Skipping...") + continue - print(f"\n{'='*60}") - print(f"๐ŸŽฌ Playing Episode {episode_idx} / {len(synced_dataset) - 1}") - print(f"{'='*60}") + if not wait_for_home(robot_controller): + print("โš ๏ธ Skipping episode due to homing failure.") + continue - episode = synced_dataset[episode_idx] + print( + f"\n{'='*60}\n๐ŸŽฌ Extracting Episode {episode_idx} / {len(synced_dataset) - 1}\n{'='*60}" + ) - print(f"\n๐Ÿš€ Collecting episode {episode_idx} data...") - rgb_frames_per_step: list[dict[str, np.ndarray]] = [] + rgb_frames_per_step = [] parallel_gripper_open_amounts = [] joint_positions = [] - for step in tqdm(episode, desc=f"Collecting episode {episode_idx}"): + + # Pre-compute and Unpack + for step in tqdm(episode, desc="Unpacking frames into memory"): 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) + j_data = step.data.get(DataType.JOINT_TARGET_POSITIONS, {}) + if all(jn in j_data for jn in JOINT_NAMES): + joint_positions.append([j_data[jn].value for jn in JOINT_NAMES]) + else: + if len(joint_positions) > 0: + joint_positions.append(joint_positions[-1]) + else: + continue + + g_data = step.data.get( + DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS, {} + ) + parallel_gripper_open_amounts.append( + g_data[GRIPPER_NAME].open_amount if GRIPPER_NAME in g_data else 0.0 + ) + + rgb_data = step.data.get(DataType.RGB_IMAGES, {}) + rgb_frames_per_step.append( + {c: img.frame for c, img in rgb_data.items()} + ) + + 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)) - 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}" - ): + # 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[index]) + + robot_controller.set_target_joint_angles(joint_positions[idx]) robot_controller.set_gripper_open_value( - parallel_gripper_open_amounts[index] + parallel_gripper_open_amounts[idx] ) - # 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 idx < len(rgb_frames_per_step): + for cam_name, frame_rgb in rgb_frames_per_step[idx].items(): + frame_bgr = cv2.cvtColor( + np.asarray(frame_rgb, dtype=np.uint8), cv2.COLOR_RGB2BGR + ) + cv2.imshow(f"Replay: {cam_name}", frame_bgr) + if cv2.waitKey(1) & 0xFF == ord("q"): - print("\n๐Ÿ›‘ 'q' pressed, stopping replay...") + print("\n๐Ÿ›‘ 'q' pressed, skipping remainder of replay...") break - end_time = time.time() - time.sleep(max(0, 1 / args.frequency - (end_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() - 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. 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() - - -if __name__ == "__main__": - main() + print("๐Ÿ‘‹ Replay complete.") diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py index 6ceb550..052c2c2 100644 --- a/examples/4_rollout_neuracore_policy.py +++ b/examples/4_rollout_neuracore_policy.py @@ -1,9 +1,18 @@ #!/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 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 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 --ik-config ik_conf/default.yaml + python 4_rollout_neuracore_policy.py --remote-endpoint-name test_deploy """ import argparse @@ -14,641 +23,98 @@ from pathlib import Path import neuracore as nc -import numpy as np -from neuracore_types import DataType, EmbodimentDescription -# Add parent directory to path to import pink_ik_solver and piper_controller +# Dynamically append the parent directory to sys.path to resolve local 'common' modules. sys.path.insert(0, str(Path(__file__).parent.parent)) +from common.config_parser import load_ik_config from common.configs import ( - CAMERA_FRAME_STREAMING_RATE, CAMERA_NAMES, - CONTROLLER_BETA, - CONTROLLER_D_CUTOFF, - CONTROLLER_DATA_RATE, - CONTROLLER_MIN_CUTOFF, - DAMPING_COST, - FRAME_TASK_GAIN, - GRIPPER_FRAME_NAME, - IK_SOLVER_RATE, - JOINT_NAMES, - JOINT_STATE_STREAMING_RATE, - LM_DAMPING, - MAX_ACTION_ERROR_THRESHOLD, - MAX_SAFETY_THRESHOLD, - NEUTRAL_JOINT_ANGLES, - ORIENTATION_COST, + GRIPPER_NAME, 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.data_manager import RobotActivityState + +# Extracted policy execution and lifecycle management actions +from common.policy_actions import ( + play_policy, + policy_execution_thread, + run_policy, + start_policy_execution, +) 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 -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 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 (Button A)") - 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 (Button A)") - 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("โš ๏ธ Button B pressed 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...") - - 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( - input_embodiment_description[DataType.RGB_IMAGES] - ) - if rgb_names: - rgb_image = data_manager.get_rgb_image(rgb_names[0]) - current_joint_angles = data_manager.get_current_joint_angles() - - # Get policy prediction - try: - start_time = time.time() - 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( - f" โœ“ Got {horizon_length} actions in {end_time - start_time:.3f} 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] - ) - joint_differences = np.abs( - current_joint_angles - np.degrees(current_joint_target_positions_rad) - ) - 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}ยฐ") - 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 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, - 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) - 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 play_policy( - data_manager: DataManager, - policy: nc.policy, - policy_state: PolicyState, - visualizer: RobotVisualizer, - input_embodiment_description: EmbodimentDescription, -) -> 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 - - policy_state.set_continuous_play_active(True) - visualizer.update_play_policy_button_status(True) - - 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, - output_gripper_names: list[str] | None, -) -> None: - """Policy execution thread.""" - dt_execution = 1.0 / POLICY_EXECUTION_RATE - # 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 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 - ): - 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 - ) - if np.any(joint_errors <= MAX_ACTION_ERROR_THRESHOLD): - break - time.sleep(0.001) - - 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 - ) - 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}" - ) - - 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() - - # Update status - 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() - end_policy_play( - data_manager, - policy_state, - visualizer, - "Continuous play stopped - prediction failed", - ) - else: - # Execution complete - print("โœ“ Policy execution completed") - end_policy_play( - data_manager, policy_state, visualizer, "Policy execution completed" - ) - - # 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_start_policy_execution_button_disabled(True) - visualizer.set_run_policy_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) - - 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_run_and_start_policy_execution_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" - ) - +from common.shared_actions import move_robot_home, toggle_robot_enabled +from common.system_bootstrap import bootstrap_robot_system +from neuracore_types import DataType if __name__ == "__main__": + # --------------------------------------------------------- + # 1. Argument Parsing + # --------------------------------------------------------- parser = argparse.ArgumentParser( - description="Piper Robot Test with Neuracore Policy - REAL ROBOT CONTROL" + 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( - "--ip-address", + "--robot-name", type=str, - default=None, - help="IP address of Meta Quest device (optional, defaults to None for auto-discovery)", + 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) policy_group.add_argument( - "--train-run-name", - type=str, - default=None, - help="Name of the training run to load policy from (for cloud training).", + "--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 model file to load policy from.", + "--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="Name of remote Neuracore policy endpoint.", - ) - parser.add_argument( - "--robot-name", - type=str, - default="AgileX PiPER", - help="Neuracore robot name (policy embodiment resolution).", + help="Active remote inference endpoint.", ) + 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...") + print("=" * 60 + "\nPIPER ROBOT TEST WITH NEURACORE POLICY\n" + "=" * 60) + + # --------------------------------------------------------- + # 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, + 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}..." @@ -661,184 +127,109 @@ def update_visualization( "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 model file: {args.model_path}...") + 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, ) - 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 + # 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 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 Meta Quest reader - print("\n๐ŸŽฎ Initializing Meta Quest reader...") - quest_reader = MetaQuestReader(ip_address=args.ip_address, port=5555, run=True) - - # Start data collection thread - print("\n๐ŸŽฎ Starting quest reader thread...") - quest_thread = threading.Thread( - target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True - ) - quest_thread.start() - - # set initial configuration to current joint angles - current_joint_angles = data_manager.get_current_joint_angles() - if current_joint_angles is not None: - initial_joint_angles = np.radians(current_joint_angles) - else: - initial_joint_angles = np.radians(NEUTRAL_JOINT_ANGLES) - - # Create Pink IK solver - print("\n๐Ÿ”ง Creating Pink IK solver...") - ik_solver = PinkIKSolver( - urdf_path=URDF_PATH, - end_effector_frame=GRIPPER_FRAME_NAME, - solver_name=SOLVER_NAME, - position_cost=POSITION_COST, - orientation_cost=ORIENTATION_COST, - frame_task_gain=FRAME_TASK_GAIN, - lm_damping=LM_DAMPING, - damping_cost=DAMPING_COST, - solver_damping_value=SOLVER_DAMPING_VALUE, - integration_time_step=1 / IK_SOLVER_RATE, - initial_configuration=initial_joint_angles, - posture_cost_vector=np.array(POSTURE_COST_VECTOR), - ) - - # Start IK solver thread - print("\n๐Ÿงฎ Starting IK solver thread...") - ik_thread = threading.Thread( - target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True - ) - ik_thread.start() - - # Start camera thread - print("\n๐Ÿ“ท Starting camera thread...") - camera_thread_obj = threading.Thread( - target=camera_thread, args=(data_manager,), daemon=True + # --------------------------------------------------------- + # 4. Hardware & Subsystem Bootstrapping + # --------------------------------------------------------- + # Instantiates the shared DataManager, CAN hardware interface, IK solver, + # and base telemetry threads using the parsed YAML config parameters. + data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( + config, start_ik=True, start_camera=True ) - camera_thread_obj.start() - # Set up visualization - print("\n๐Ÿ–ฅ๏ธ Starting Viser visualization...") + # --------------------------------------------------------- + # 5. Visualization & UI Setup + # --------------------------------------------------------- + print("\n๐Ÿ–ฅ๏ธ Starting Viser visualization server...") visualizer = RobotVisualizer(str(URDF_PATH)) + + # Inject UI components using fixed AI policy rates 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, + 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 + # Bind hardware action callbacks to the UI buttons visualizer.set_toggle_robot_enabled_status_callback( - lambda: toggle_robot_enabled_status(data_manager, robot_controller, visualizer) + lambda: toggle_robot_enabled(data_manager, robot_controller, visualizer) + ) + visualizer.set_go_home_callback( + lambda: move_robot_home(data_manager, robot_controller) ) - visualizer.set_go_home_callback(lambda: home_robot(data_manager, robot_controller)) + + # Bind policy execution callbacks to the UI buttons visualizer.set_run_policy_callback( - lambda: run_policy( - data_manager, - policy, - policy_state, - visualizer, - input_embodiment_description, - ) + 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_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, policy, policy_state, visualizer, - input_embodiment_description, + input_emb, + 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 (after visualizer is created) - quest_reader.on( - "button_a_pressed", - lambda: toggle_robot_enabled_status(data_manager, robot_controller, visualizer), - ) - quest_reader.on( - "button_b_pressed", lambda: home_robot(data_manager, robot_controller) - ) - - # Start policy execution thread - print("\n๐Ÿค– Starting policy execution thread...") - policy_execution_thread_obj = threading.Thread( + # --------------------------------------------------------- + # 6. Dispatch Policy Execution Thread + # --------------------------------------------------------- + policy_exec_thread = threading.Thread( target=policy_execution_thread, args=( policy, @@ -846,56 +237,51 @@ def update_visualization( policy_state, robot_controller, visualizer, - input_embodiment_description, - output_gripper_names, + input_emb, ), daemon=True, + name="PolicyExecutionWorker", + ) + policy_exec_thread.start() + active_threads.append(policy_exec_thread) + + print( + "\n๐Ÿš€ System Online! Open http://localhost:8080 in your browser to visualize and run the policy.\n" ) - policy_execution_thread_obj.start() - - print() - print("๐Ÿš€ Starting teleoperation with policy testing...") - print("๐ŸŽฎ CONTROLS:") - print(" 1. Press BUTTON A or Enable Robot button to enable/disable robot") - print(" 2. You have same control over the robot as in teleoperation.") - print(" - Hold RIGHT GRIP to activate teleoperation") - print(" - Move controller - robot follows!") - print(" - Hold RIGHT TRIGGER to close gripper") - print(" - Press BUTTON A or Enable Robot button to enable/disable robot") - print(" - Press BUTTON B or Home Robot button to send robot home") - print(" 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("โš ๏ธ Press Ctrl+C to exit") - print() - print("๐ŸŒ Open browser: http://localhost:8080") + # --------------------------------------------------------- + # 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๐Ÿ‘‹ Interrupt received - 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() - # Cleanup - print("\n๐Ÿงน Cleaning up...") + # --------------------------------------------------------- + # 8. Graceful Teardown & Cleanup + # --------------------------------------------------------- + print("\n๐Ÿงน Cleaning up subsystems...") - # Disconnect policy + # Safely sever the connection to the policy backend policy.disconnect() - # shutdown threads + # Broadcast shutdown signal to all worker threads data_manager.request_shutdown() data_manager.set_robot_activity_state(RobotActivityState.DISABLED) - quest_thread.join() - quest_reader.stop() - ik_thread.join() - camera_thread_obj.join() - robot_controller.cleanup() + # 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("\n๐Ÿ‘‹ Demo stopped.") + print("๐Ÿ‘‹ Shutdown complete. Goodbye.") diff --git a/examples/5_rollout_neuracore_policy_minimal.py b/examples/5_rollout_neuracore_policy_minimal.py index 132cde6..0e0134f 100644 --- a/examples/5_rollout_neuracore_policy_minimal.py +++ b/examples/5_rollout_neuracore_policy_minimal.py @@ -1,35 +1,48 @@ #!/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 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 threading import time import traceback from pathlib import Path +from typing import Any import neuracore as nc -# Add parent directory to path +# --------------------------------------------------------------------------- +# Path Configuration & Local Imports +# --------------------------------------------------------------------------- sys.path.insert(0, str(Path(__file__).parent.parent)) +from common.config_parser import load_ik_config from common.configs import ( - NEUTRAL_JOINT_ANGLES, + CAMERA_NAMES, + GRIPPER_NAME, POLICY_EXECUTION_RATE, PREDICTION_HORIZON_EXECUTION_RATIO, - ROBOT_RATE, URDF_PATH, ) -from common.data_manager import DataManager, RobotActivityState +from common.data_manager import RobotActivityState +from common.policy_actions import run_policy from common.policy_helpers import ( - convert_predictions_to_horizon, embodiment_names_ordered, get_policy_embodiments, gripper_open_at_index, @@ -38,53 +51,22 @@ 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 +from common.system_bootstrap import bootstrap_robot_system +from neuracore_types import DataType +# --------------------------------------------------------------------------- +# Helper Functions +# --------------------------------------------------------------------------- def execute_horizon( - data_manager: DataManager, + data_manager: Any, policy_state: PolicyState, - robot_controller: PiperController, + robot_controller: Any, frequency: int, - input_embodiment_description: EmbodimentDescription, - output_gripper_names: list[str] | None, + input_embodiment: dict, + output_grippers: list[str] | None, ) -> None: - """Execute prediction horizon.""" + """Minimal terminal-only execution loop for the prediction horizon.""" policy_state.start_policy_execution() data_manager.set_robot_activity_state(RobotActivityState.POLICY_CONTROLLED) @@ -95,237 +77,219 @@ def execute_horizon( 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_gripper_names + 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 state back to the policy for the next inference cycle + log_robot_state_for_policy(data_manager, input_embodiment) - # Sleep to maintain rate - elapsed = time.time() - start_time - time.sleep(max(0, dt - elapsed)) + # Maintain loop frequency + time.sleep(max(0.0, dt - (time.time() - start_time))) - # End execution policy_state.end_policy_execution() data_manager.set_robot_activity_state(RobotActivityState.ENABLED) +# --------------------------------------------------------------------------- +# Main Execution +# --------------------------------------------------------------------------- if __name__ == "__main__": parser = argparse.ArgumentParser(description="Minimal Piper Policy Test") + policy_group = parser.add_mutually_exclusive_group(required=True) policy_group.add_argument( - "--train-run-name", - type=str, - default=None, - help="Name of the training run to load policy from (for cloud training).", + "--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 model file to load policy from.", + "--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="Name of remote Neuracore policy endpoint.", - ) - parser.add_argument( - "--robot-name", - type=str, - default="AgileX PiPER", - help="Neuracore robot name (policy embodiment resolution).", + 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( - "--frequency", - type=int, - default=POLICY_EXECUTION_RATE, - help="Frequency of policy execution", + "--execution-ratio", type=float, default=PREDICTION_HORIZON_EXECUTION_RATIO ) parser.add_argument( - "--execution-ratio", - type=float, - default=PREDICTION_HORIZON_EXECUTION_RATIO, - help="Execution ratio of the policy", + "--ik-config", + type=str, + default="ik_conf/default.yaml", + help="Path to YAML configuration.", ) 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. 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, + robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False ) - if args.remote_endpoint_name is not None: + # 2. Policy Loading + if 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: + except Exception: + # Catch the EndpointError and provide explicit, actionable instructions + print("\n" + "!" * 60) + print(f"โŒ ENDPOINT NOT ACTIVE: '{args.remote_endpoint_name}'") + print("!" * 60) + print( + "The script successfully reached Neuracore, but the remote server is down." + ) + print("\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( - f"โŒ Endpoint '{args.remote_endpoint_name}' not available. " - "Please start it from the Neuracore dashboard." + " 4. Wait for the status to show as 'Active', then rerun this script." ) + print("!" * 60 + "\n") sys.exit(1) - elif args.train_run_name is not None: - print(f"\n๐Ÿค– Loading policy from training run: {args.train_run_name}...") + + 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 model file: {args.model_path}...") + 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, - ) - 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 + model_file=args.model_path, device="cuda", robot_name=args.robot_name ) - 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() + # 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: list[str] | None = None + if output_emb and DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in output_emb: + output_gripper_names = embodiment_names_ordered( + output_emb[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS] + ) - # 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 + # 4. Bootstrap Core System (No Quest, No IK needed for minimal playback) + data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( + config, start_ik=False, start_camera=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() + policy_state = PolicyState() + policy_state.set_execution_ratio(args.execution_ratio) - # Wait for threads to initialize - print("\nโณ Waiting for initialization...") - time.sleep(2.0) + time.sleep(2.0) # Wait for hardware interfaces and threads to initialize + # 5. Continuous Execution Loop 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) + 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") - # 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" + ) while True: - # Run policy + # Query the network for the next prediction if not run_policy( - data_manager, policy, policy_state, input_embodiment_description + 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 + # Execute the predicted horizon on the hardware execute_horizon( data_manager, policy_state, robot_controller, args.frequency, - input_embodiment_description, + input_emb, output_gripper_names, ) except KeyboardInterrupt: - print("\n\n๐Ÿ‘‹ Interrupt received - 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: - # 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") + 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 - # 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() + for t in active_threads: + t.join(timeout=2.0) - print("โœ“ Cleanup complete") - print("\n๐Ÿ‘‹ Done.") + if robot_controller: + try: + robot_controller.graceful_stop() + except AttributeError: + pass + robot_controller.cleanup() + + nc.logout() + print("๐Ÿ‘‹ Shutdown complete.") diff --git a/examples/6_visualize_policy_from_dataset.py b/examples/6_visualize_policy_from_dataset.py index 64d7f7e..9e09dda 100644 --- a/examples/6_visualize_policy_from_dataset.py +++ b/examples/6_visualize_policy_from_dataset.py @@ -1,275 +1,321 @@ #!/usr/bin/env python3 -"""Simple policy visualization from dataset. +"""Offline Policy Visualization and Validation. -Loads a policy and a dataset, and randomly selects a state -from the dataset to run the policy with and visualize the results. +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 logging import random import sys import time import traceback from pathlib import Path +from typing import Any + +sys.path.insert(0, str(Path(__file__).parent.parent)) 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)) +# --------------------------------------------------------------------------- +# 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) + +if __name__ == "__main__": + # --------------------------------------------------------------------------- + # Path Configuration & Local Imports + # --------------------------------------------------------------------------- + sys.path.insert(0, str(Path(__file__).parent.parent)) + + from common.configs import ( + CAMERA_NAMES, + GRIPPER_NAME, + JOINT_NAMES, + POLICY_EXECUTION_RATE, + URDF_PATH, + ) + from common.dataset_helpers import load_and_sync_dataset + from common.policy_helpers import ( + DEFAULT_ROBOT_NAME, + convert_predictions_to_horizon, + embodiment_names_ordered, + get_policy_embodiments, + gripper_open_at_index, + horizon_length, + log_sync_step_for_policy, + print_policy_embodiments, + urdf_cfg_from_horizon, + ) + from neuracore_types import DataType + + # --------------------------------------------------------- + # 1. Argument Parsing + # --------------------------------------------------------- + parser = argparse.ArgumentParser( + description="Visualize policy predictions offline using dataset states." + ) + parser.add_argument( + "--dataset-name", + type=str, + required=True, + help="Neuracore dataset to draw observations from.", + ) + + policy_group = parser.add_mutually_exclusive_group(required=True) + policy_group.add_argument( + "--train-run-name", type=str, default=None, help="Cloud training run name." + ) + policy_group.add_argument( + "--model-path", type=str, default=None, help="Path to local .nc.zip model file." + ) + policy_group.add_argument( + "--remote-endpoint-name", + type=str, + default=None, + help="Active remote inference endpoint.", + ) + + 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: + print( + f"\n๐Ÿค– Connecting to remote policy endpoint: {args.remote_endpoint_name}..." + ) + try: + policy = nc.policy_remote_server(args.remote_endpoint_name) + except Exception: + print("\n" + "!" * 60) + print(f"โŒ ENDPOINT NOT ACTIVE: '{args.remote_endpoint_name}'") + print("!" * 60) + print( + "The script successfully reached Neuracore, but the remote server is down." + ) + print("\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 + ) -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}...") + # --------------------------------------------------------- + # 3. Embodiment Extraction & Fallback + # --------------------------------------------------------- try: - policy = nc.policy_remote_server(args.remote_endpoint_name) - except nc.EndpointError: + input_emb, output_emb = get_policy_embodiments(policy) + except AttributeError: print( - f"โŒ Endpoint '{args.remote_endpoint_name}' not available. " - "Please start it from the Neuracore dashboard." + "\nโš ๏ธ Could not dynamically extract embodiments. Using default Piper configuration..." ) - 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, + 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 ) -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, + + # --------------------------------------------------------- + # 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(" โœ“ 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 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" ) - 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, + urdf_vis.update_cfg(np.zeros(len(JOINT_NAMES))) + + state: dict[str, Any] = { + "horizon": None, + "action_idx": 0, + "playing": False, + "rgb_handle": None, + } + + def select_random_state() -> None: + """Pulls a random observation from the dataset and queries the AI policy.""" + 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_emb): + return + + rgb_data = getattr(step, "data", {}).get(DataType.RGB_IMAGES, {}) + for _, frame in rgb_data.items(): + rgb_arr = np.array(frame.frame) + Image.fromarray(rgb_arr).save("current_image.png") + + if state["rgb_handle"] is None: + state["rgb_handle"] = server.gui.add_image( + rgb_arr, label="RGB Observation", format="jpeg" ) else: - rgb_gui_handle.image = rgb_image + state["rgb_handle"].image = rgb_arr break - print("๐ŸŽฏ Getting policy prediction...") - start_time = time.time() + try: + 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) + + print( + f"โœ“ Prediction successful. Horizon length: {horizon_length(state['horizon'])}" + ) + except Exception as e: + print(f"โŒ Failed prediction inference: {e}") + + server.gui.add_button("๐ŸŽฒ Pull Random Observation").on_click( + lambda _: select_random_state() + ) + gripper_handle = server.gui.add_slider( + "Predicted Gripper State", + min=0.0, + max=1.0, + step=0.01, + initial_value=0.0, + disabled=True, + ) + freq_handle = server.gui.add_number( + "Simulation Speed (Hz)", + initial_value=args.frequency, + min=1.0, + max=500.0, + step=1.0, + ) + + 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: - 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)} + 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) - 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) + state["action_idx"] = (state["action_idx"] + 1) % h_len - current_action_idx = (current_action_idx + 1) % h_len + time.sleep( + max(0.0, 1.0 / max(freq_handle.value, 0.1) - (time.time() - start_time)) + ) - elapsed = time.time() - start_time - frequency = max(frequency_handle.value, 0.1) - time.sleep(max(0, 1.0 / frequency - elapsed)) + except KeyboardInterrupt: + print("\n๐Ÿ‘‹ Interrupt received. Shutting down gracefully...") + except Exception as e: + print(f"\nโŒ Unhandled error in rendering loop: {e}") + traceback.print_exc() -except KeyboardInterrupt: - print("\n๐Ÿ‘‹ Shutting down...") -finally: - policy.disconnect() - nc.logout() + # --------------------------------------------------------- + # 7. Cleanup + # --------------------------------------------------------- + finally: + print("\n๐Ÿงน Severing backend connections...") + policy.disconnect() + nc.logout() + print("๐Ÿ‘‹ Offline validation complete.") diff --git a/examples/7_teleop_with_pedal.py b/examples/7_teleop_with_pedal.py index 4b00a1f..67d2d0a 100644 --- a/examples/7_teleop_with_pedal.py +++ b/examples/7_teleop_with_pedal.py @@ -1,8 +1,24 @@ #!/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 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 @@ -10,270 +26,203 @@ 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 +# --------------------------------------------------------------------------- +# Path Configuration & Local Imports +# --------------------------------------------------------------------------- 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.config_parser import load_ik_config +from common.configs import META_QUEST_AXIS_MASK, URDF_PATH +from common.data_manager import 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.shared_actions import ( + move_robot_home, + neuracore_logging_callback, + toggle_recording, + toggle_robot_enabled, +) +from common.system_bootstrap import bootstrap_robot_system from common.threads.quest_reader import quest_reader_thread from meta_quest_teleop.reader import MetaQuestReader -from pink_ik_solver import PinkIKSolver -from piper_controller import PiperController - +# Hardware key mappings for the USB foot pedal 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__": + # Ensure safe multiprocess spawning for UI/Background threads 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") + # --------------------------------------------------------- + # 1. Argument Parsing + # --------------------------------------------------------- + parser = argparse.ArgumentParser(description="Combined Quest + Pedal Teleop") + parser.add_argument( + "--ip-address", + type=str, + default=None, + help="Meta Quest IP address on the local network.", + ) + parser.add_argument( + "--dataset-name", + type=str, + default=None, + help="Override the auto-generated dataset name.", + ) + parser.add_argument( + "--ik-config", + type=str, + default="ik_conf/default.yaml", + help="Path to IK/teleop YAML config.", + ) 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) + + # Load the YAML configuration dictionary + config = load_ik_config(args.ik_config) - # Neuracore Init + # --------------------------------------------------------- + # 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 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 + # --------------------------------------------------------- + # 3. Hardware & Subsystem Bootstrapping + # --------------------------------------------------------- + data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system( + config, start_ik=True, start_camera=True ) - # 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() + # Wire Neuracore logging to DataManager so states push to the cloud + data_manager.set_on_change_callback(neuracore_logging_callback) + # --------------------------------------------------------- + # 4. Meta Quest Initialization & Error Handling + # --------------------------------------------------------- + print("\n๐ŸŽฎ Initializing Meta 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( + 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 - ).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), + quest_thread.start() + active_threads.append(quest_thread) + except (Exception, SystemExit): + 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." ) - 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}") + print(" 4. Rerun this script.") + print("!" * 60 + "\n") - try: - threading.Thread( - target=camera_thread, args=(data_manager,), daemon=True - ).start() - except Exception as e: - print(f"โš ๏ธ Camera thread failed to start: {e}") + data_manager.request_shutdown() + for t in active_threads: + t.join(timeout=1.0) + if robot_controller: + robot_controller.cleanup() + sys.exit(1) - # Foot Pedal โ€“ started as a daemon thread, callbacks wired inline + # --------------------------------------------------------- + # 5. Foot Pedal Initialization & 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) + + # Bind the hardware actions to the pedal inputs + pedal.bind("button_a", lambda: toggle_robot_enabled(data_manager, robot_controller)) + pedal.bind("button_b", lambda: move_robot_home(data_manager, robot_controller)) + pedal.bind("button_c", lambda: toggle_recording(play_audio=False)) pedal_thread = threading.Thread( target=pedal.run_loop, args=(data_manager,), daemon=True ) pedal_thread.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 arm, TRIGGER to close gripper") + print("โŒจ๏ธ PEDAL CONTROLS: ENABLE/DISABLE (Left), HOME (Middle), RECORD (Right)\n") + # --------------------------------------------------------- + # 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 pedal_thread: - pedal_thread.join(timeout=1.0) + print("\n๐Ÿงน Cleaning up subsystems...") + + # Ensure we don't leave dangling, unterminated recordings on the cloud + if nc.is_recording(): + nc.cancel_recording() + try: - if nc.is_recording(): - nc.cancel_recording() nc.logout() except Exception: pass + + data_manager.set_robot_activity_state(RobotActivityState.DISABLED) data_manager.request_shutdown() + if quest_reader: - try: - quest_reader.stop() - except Exception: - pass + quest_reader.stop() + + for t in active_threads: + t.join(timeout=2.0) + if robot_controller: - try: - robot_controller.cleanup() - except Exception: - pass + robot_controller.cleanup() + + print("๐Ÿ‘‹ Demo stopped.") diff --git a/examples/common/config_parser.py b/examples/common/config_parser.py new file mode 100644 index 0000000..bf1c8e2 --- /dev/null +++ b/examples/common/config_parser.py @@ -0,0 +1,18 @@ +"""Utilities for loading teleoperation and IK configuration from YAML files.""" + +from pathlib import Path + +import yaml # type: ignore[import] + + +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) diff --git a/examples/common/configs.py b/examples/common/configs.py index e8142b0..5d7314f 100644 --- a/examples/common/configs.py +++ b/examples/common/configs.py @@ -1,80 +1,49 @@ -"""Configuration parameters for Piper robot demos.""" +"""Configuration parameters for AgileX Piper robot demos.""" from pathlib import Path +# Hardware & Kinematics Paths URDF_PATH = str( Path(__file__).parent.parent.parent / "piper_description" / "urdf" / "piper_description.urdf" ) - 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.0 -DAMPING_COST = 0.25 -SOLVER_DAMPING_VALUE = 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_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 -SLOW_TRANSLATION_SCALE = 0.6 -SLOW_ROTATION_SCALE = 0.6 -WRIST_JOINT_BUTTON_STEP_DEGREES = 5.0 +# Teleoperation Thresholds +GRIP_THRESHOLD = 0.9 -# Thread rates (Hz) -CONTROLLER_DATA_RATE = 50.0 # Controller input reading -IK_SOLVER_RATE = 250.0 # IK solving and robot commands -VISUALIZATION_RATE = 60.0 # GUI updates +# 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 -# 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 - -# 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] +# 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 = 20.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/data_manager.py b/examples/common/data_manager.py index fec186d..da0461c 100644 --- a/examples/common/data_manager.py +++ b/examples/common/data_manager.py @@ -4,7 +4,7 @@ This module provides shared state classes for teleoperation systems that need to coordinate between multiple threads (data collection, IK solving, visualization). """ - +import queue # Added for async queueing import threading import time from enum import Enum @@ -112,30 +112,67 @@ class DataManager: """ 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() self._ik_state = IKState() self._camera_state = CameraState() - # 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] ) -> 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 # ============================================================================ @@ -145,28 +182,25 @@ 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) - """ + """Return the latest controller pose, grip, and trigger values.""" with self._controller_state._lock: return ( ( @@ -181,18 +215,7 @@ def get_controller_data(self) -> tuple[np.ndarray | None, float, float]: def set_controller_data( self, transform: np.ndarray | None, grip: float, trigger: float ) -> None: - """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 - """ + """Update the controller transform and button values.""" if transform is not None and transform.shape != (4, 4): raise ValueError("Transform must be a 4x4 matrix") if grip < 0.0 or grip > 1.0: @@ -206,11 +229,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 +241,30 @@ 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 - """ + """Set the filter parameters used to smooth controller motion.""" with self._controller_state._lock: self._controller_state.min_cutoff = min_cutoff self._controller_state.beta = beta self._controller_state.d_cutoff = d_cutoff def get_controller_filter_params(self) -> tuple[float, float, float]: - """Get 1โ‚ฌ Filter parameters for controller transform (thread-safe). - - Returns: - Tuple of (min_cutoff, beta, d_cutoff) - """ + """Return the current controller filter parameters.""" with self._controller_state._lock: return ( self._controller_state.min_cutoff, @@ -277,13 +282,7 @@ 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 - """ + """Enable or disable teleoperation and store initial transforms.""" with self._teleop_state._lock: self._teleop_state.active = active self._teleop_state.controller_initial_transform = ( @@ -296,26 +295,15 @@ 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. + """Update the teleoperation scaling factors.""" 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) - """ + """Return the current teleoperation scaling settings.""" with self._teleop_state._lock: return ( self._teleop_state.translation_scale, @@ -323,17 +311,17 @@ def get_teleop_scaling(self) -> tuple[float, float]: ) def get_teleop_active(self) -> bool: - """Get teleoperation active state (thread-safe).""" + """Return whether teleoperation is currently active.""" with self._teleop_state._lock: return self._teleop_state.active def set_slow_scaling_mode_enabled(self, enabled: bool) -> None: - """Set slow-scaling mode status (thread-safe).""" + """Enable or disable slow scaling mode.""" with self._teleop_state._lock: self._teleop_state.slow_scaling_mode_enabled = enabled def toggle_slow_scaling_mode_enabled(self) -> bool: - """Toggle and return slow-scaling mode status (thread-safe).""" + """Toggle slow scaling mode and return the new state.""" with self._teleop_state._lock: self._teleop_state.slow_scaling_mode_enabled = ( not self._teleop_state.slow_scaling_mode_enabled @@ -341,21 +329,14 @@ def toggle_slow_scaling_mode_enabled(self) -> bool: return self._teleop_state.slow_scaling_mode_enabled def get_slow_scaling_mode_enabled(self) -> bool: - """Get slow-scaling mode status (thread-safe).""" + """Return whether slow scaling mode is enabled.""" with self._teleop_state._lock: return self._teleop_state.slow_scaling_mode_enabled def get_initial_robot_controller_transforms( self, ) -> tuple[np.ndarray | None, np.ndarray | None]: - """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) - """ + """Return the initial controller and robot transforms for teleop.""" with self._teleop_state._lock: return ( ( @@ -375,29 +356,17 @@ 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 - """ + """Return the current robot activity state.""" with self._robot_state._lock: return self._robot_state.activity_state def set_robot_activity_state(self, state: RobotActivityState) -> None: - """Set robot activity state (thread-safe). - - Args: - state: RobotActivityState - new robot activity state - """ + """Change the robot activity state.""" with self._robot_state._lock: self._robot_state.activity_state = state def get_current_joint_angles(self) -> np.ndarray | None: - """Get current joint angles (thread-safe). - - Returns: - Current joint angles or None if not available - """ + """Return the current robot joint angles in degrees.""" with self._robot_state._lock: return ( self._robot_state.joint_angles.copy() @@ -406,11 +375,7 @@ 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 - """ + """Update the current joint angles and log them if callbacks are enabled.""" with self._robot_state._lock: self._robot_state.joint_angles = angles.copy() if self._on_change_callback: @@ -419,14 +384,10 @@ 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 - """ + """Return the current robot joint torques.""" with self._robot_state._lock: return ( self._robot_state.joint_torques.copy() @@ -435,25 +396,17 @@ 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 - """ + """Update current robot joint torque readings.""" 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 - """ + """Return the current end effector pose.""" with self._robot_state._lock: return ( self._robot_state.end_effector_pose.copy() @@ -462,61 +415,37 @@ 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 - """ + """Update the current end effector pose.""" with self._robot_state._lock: self._robot_state.end_effector_pose = pose.copy() def get_current_gripper_open_value(self) -> float | None: - """Get current gripper open value (thread-safe). - - Returns: - Current gripper open value or None if not available - """ + """Return the current gripper open amount.""" 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 - """ + """Update the 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 - """ + """Return the target gripper open amount.""" with self._robot_state._lock: return self._robot_state.target_gripper_open_value def set_target_gripper_open_value(self, value: float) -> None: - """Set target gripper open value (thread-safe). - - Args: - value: float - target gripper open value - """ + """Update the target gripper open amount.""" 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 +456,7 @@ 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 - """ + """Return the latest target joint angles from the IK solver.""" with self._ik_state._lock: return ( self._ik_state.target_joint_angles.copy() @@ -540,11 +465,7 @@ 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 - """ + """Store the target joint angle command from the IK solver.""" with self._ik_state._lock: self._ik_state.target_joint_angles = angles.copy() if self._on_change_callback: @@ -553,27 +474,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( - "log_joint_target_positions", payload, time.time() - ) + self._queue_callback("log_joint_target_positions", payload, time.time()) def set_target_pose(self, transform: np.ndarray | None) -> None: - """Set target transform for visualization (thread-safe). - - Args: - transform: np.ndarray | None - 4x4 transformation matrix or None to clear target transform - """ + """Store the latest IK target end effector pose.""" with self._ik_state._lock: self._ik_state.target_pose = ( transform.copy() if transform is not None else None ) def get_target_pose(self) -> np.ndarray | None: - """Get target transform for visualization (thread-safe). - - Returns: - Target transform or None if target transform is not set - """ + """Return the latest IK target pose.""" with self._ik_state._lock: return ( self._ik_state.target_pose.copy() @@ -582,38 +493,22 @@ 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 - """ + """Record the latest IK solve duration in milliseconds.""" with self._ik_state._lock: self._ik_state.solve_time_ms = time_ms def set_ik_success(self, success: bool) -> None: - """Set IK success (thread-safe). - - Args: - success: bool - True if IK was successful, False otherwise - """ + """Record whether the last IK solve succeeded.""" with self._ik_state._lock: self._ik_state.success = success def get_ik_solve_time_ms(self) -> float: - """Get IK solve time (thread-safe). - - Returns: - IK solve time in milliseconds - """ + """Return the last IK solve time in milliseconds.""" with self._ik_state._lock: return self._ik_state.solve_time_ms def get_ik_success(self) -> bool: - """Get IK success (thread-safe). - - Returns: - True if IK was successful, False otherwise - """ + """Return whether the last IK solve succeeded.""" with self._ik_state._lock: return self._ik_state.success @@ -622,13 +517,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 - """ + """Check if shutdown is requested.""" return self._shutdown_event.is_set() diff --git a/examples/common/dataset_helpers.py b/examples/common/dataset_helpers.py new file mode 100644 index 0000000..6519e31 --- /dev/null +++ b/examples/common/dataset_helpers.py @@ -0,0 +1,57 @@ +"""Helpers for loading and synchronizing Neuracore datasets.""" + +from typing import Any + +import neuracore as nc +from neuracore.core.utils.robot_data_spec_utils import ( + merge_cross_embodiment_description, +) +from neuracore_types import CrossEmbodimentDescription, DataType + + +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, +) -> Any: + """Loads a Neuracore dataset and synchronizes the specified modalities across embodiments.""" + print(f"\n๐Ÿ” Getting dataset '{dataset_name}' from Neuracore...") + dataset = nc.get_dataset(dataset_name) + + 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...") + + # 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 diff --git a/examples/common/policy_actions.py b/examples/common/policy_actions.py new file mode 100644 index 0000000..3d273ed --- /dev/null +++ b/examples/common/policy_actions.py @@ -0,0 +1,385 @@ +"""Extracted actions for running and executing policies across different rollout scripts.""" + +import threading +import time +from typing import Any, Optional + +import numpy as np +from common.configs import ( + CAMERA_NAMES, + GRIPPER_NAME, + JOINT_NAMES, + MAX_ACTION_ERROR_THRESHOLD, + MAX_SAFETY_THRESHOLD, + POLICY_EXECUTION_RATE, + TARGETING_POSE_TIME_THRESHOLD, +) +from common.data_manager import DataManager, RobotActivityState +from common.policy_helpers import ( + convert_predictions_to_horizon, + log_robot_state_for_policy, +) +from common.policy_state import PolicyState + + +def run_policy( + data_manager: DataManager, + policy: Any, + policy_state: PolicyState, + visualizer: Optional[Any], + input_embodiment_description: dict, +) -> 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: DataManager, + policy: Any, + policy_state: PolicyState, + visualizer: Optional[Any], + input_emb: dict, + continuous_mode: str, +) -> None: + """Background thread for continuous receding horizon execution.""" + colors = [ + (1.0, 0.65, 0.0, 0.25), + (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: DataManager, + policy: Any, + policy_state: PolicyState, + visualizer: Optional[Any], + input_emb: dict, + continuous_mode: str = "pipeline", +) -> None: + """Toggle for starting/stopping continuous policy mode.""" + if not policy_state.get_continuous_play_active(): + print(f"โ–ถ๏ธ Starting {continuous_mode.capitalize()} Mode...") + 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: Any, + data_manager: DataManager, + policy_state: PolicyState, + robot_controller: Any, + visualizer: Optional[Any], + input_emb: dict, +) -> None: + """The thread that continuously reads the locked horizon and sends joint commands.""" + dt_execution = 1.0 / POLICY_EXECUTION_RATE + last_vis_update = 0.0 + + 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) diff --git a/examples/common/policy_state.py b/examples/common/policy_state.py index 42e90b6..b75e035 100644 --- a/examples/common/policy_state.py +++ b/examples/common/policy_state.py @@ -20,7 +20,9 @@ def __init__(self) -> None: # Prediction horizon stored as dict[str, list[float]] where keys are joint/gripper names self._prediction_horizon: dict[str, list[float]] = {} self._prediction_horizon_lock = threading.Lock() - self._execution_ratio: float = 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() @@ -90,14 +92,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 +106,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..c34bdf5 100644 --- a/examples/common/robot_visualizer.py +++ b/examples/common/robot_visualizer.py @@ -1,890 +1,33 @@ #!/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 +from typing import Any -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=(0.2, 0.4, 1.0, 0.25), # Blue with 60% 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._run_and_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") - self._start_policy_execution_button = self._server.gui.add_button( - "Start Policy Execution" - ) - 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") - - 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_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. - - 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_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. - - Args: - disabled: Whether button should be disabled - """ - if self._play_policy_button is not None: - self._play_policy_button.disabled = disabled + """Initialize the visualizer components.""" + self._core = RobotVisualizerCore(urdf_path) + self._gui = RobotVisualizerGUI(self._core.server) - def update_play_policy_button_status(self, active: bool) -> None: - """Update play policy button label based on active state. + def __getattr__(self, name: str) -> Any: + """Dynamically delegates method calls to the Core or GUI modules. - Args: - active: Whether continuous play is currently active + If a top-level script calls `visualizer.add_basic_controls()`, + this routes it to `_gui.add_basic_controls()`. """ - if self._play_policy_button is not None: - self._play_policy_button.label = "Stop Policy" if active else "Play Policy" + if hasattr(self._core, name): + return getattr(self._core, name) + if hasattr(self._gui, name): + return getattr(self._gui, name) - def stop(self) -> None: - """Stop the visualizer server.""" - self._server.stop() + raise AttributeError(f"'RobotVisualizer' object has no attribute '{name}'") diff --git a/examples/common/shared_actions.py b/examples/common/shared_actions.py new file mode 100644 index 0000000..cd64537 --- /dev/null +++ b/examples/common/shared_actions.py @@ -0,0 +1,111 @@ +"""Common robot action helpers shared by example scripts.""" + +import subprocess +from typing import Any + +import neuracore as nc +from common.data_manager import DataManager, RobotActivityState + +from piper_controller import PiperController + + +def toggle_robot_enabled( + data_manager: DataManager, + robot_controller: PiperController, + visualizer: Any = None, +) -> 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 +) -> None: + """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}") diff --git a/examples/common/states.py b/examples/common/states.py new file mode 100644 index 0000000..5c5971b --- /dev/null +++ b/examples/common/states.py @@ -0,0 +1,81 @@ +"""State containers used by the Piper example suite.""" + +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: + """Initialize controller input state and filtering state.""" + 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: + """Initialize teleop activation and scaling state.""" + 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: + """Initialize robot telemetry and activity state.""" + 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 IK target and status information.""" + 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 camera image storage.""" + self._lock = threading.Lock() + self.rgb_images: dict[str, np.ndarray] = {} diff --git a/examples/common/system_bootstrap.py b/examples/common/system_bootstrap.py new file mode 100644 index 0000000..c8beee8 --- /dev/null +++ b/examples/common/system_bootstrap.py @@ -0,0 +1,110 @@ +"""Bootstrap robot subsystems for the AgileX Piper example suite.""" + +import threading +from typing import List, Optional, Tuple + +import numpy as np +from common.configs import ( + GRIPPER_FRAME_NAME, + IK_SOLVER_RATE, + NEUTRAL_END_EFFECTOR_POSE, + NEUTRAL_JOINT_ANGLES, + ROBOT_RATE, + SOLVER_NAME, + URDF_PATH, +) +from common.data_manager import DataManager +from common.threads.ik_solver import ik_solver_thread +from common.threads.joint_state import joint_state_thread +from common.threads.realsense_camera import camera_thread + +from pink_ik_solver import PinkIKSolver +from piper_controller import PiperController + + +def bootstrap_robot_system( + config: dict, start_ik: bool = True, start_camera: bool = True +) -> Tuple[ + DataManager, PiperController, Optional[PinkIKSolver], List[threading.Thread] +]: + """Create and start the shared robot subsystems used by deployment scripts.""" + # Extract config sections safely + filt_p = config.get("filter_parameters", {}) + tele_p = config.get("teleop_parameters", {}) + ik_p = config.get("ik_parameters", {}) + + # 1. Initialize Data Manager + data_manager = DataManager() + data_manager.set_controller_filter_params( + filt_p.get("controller_min_cutoff", 0.8), + filt_p.get("controller_beta", 0.05), + filt_p.get("controller_d_cutoff", 0.9), + ) + data_manager.set_teleop_scaling( + tele_p.get("translation_scale", 1.5), tele_p.get("rotation_scale", 1.2) + ) + + # 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=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( + 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.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 diff --git a/examples/common/threads/realsense_camera.py b/examples/common/threads/realsense_camera.py index 24e6dfe..eee28b6 100644 --- a/examples/common/threads/realsense_camera.py +++ b/examples/common/threads/realsense_camera.py @@ -1,7 +1,8 @@ -"""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 numpy as np import pyrealsense2 as rs @@ -10,13 +11,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[float] = deque(maxlen=100) + last_frame_time = time.time() + try: # Configure RealSense pipeline pipeline = rs.pipeline() @@ -34,26 +43,71 @@ 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}") diff --git a/examples/common/visualizer_core.py b/examples/common/visualizer_core.py new file mode 100644 index 0000000..c71b1be --- /dev/null +++ b/examples/common/visualizer_core.py @@ -0,0 +1,112 @@ +"""Module-level helpers for the robot visualizer.""" + +from typing import Any + +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: + """TODO: Document this method.""" + 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: Any = None + self.target_frame_handle: Any = None + self.rgb_image_handle: Any = None + + def add_controller_visualization(self) -> None: + """TODO: Document this method.""" + self.controller_handle = self.server.scene.add_transform_controls( + "/controller", scale=0.15, position=(0, 0, 0), wxyz=(1, 0, 0, 0) + ) + + def add_target_frame_visualization(self) -> None: + """TODO: Document this method.""" + self.target_frame_handle = self.server.scene.add_frame( + "/target_goal", axes_length=0.1, axes_radius=0.003 + ) + + def add_rgb_image_placeholder(self, height: int = 480, width: int = 640) -> None: + """TODO: Document this method.""" + if self.rgb_image_handle is None: + dummy_image = np.zeros((height, width, 3), dtype=np.uint8) + self.rgb_image_handle = self.server.gui.add_image( + dummy_image, label="RGB Camera", format="jpeg", jpeg_quality=85 + ) + + def update_rgb_image(self, rgb_image: np.ndarray | None) -> None: + """TODO: Document this method.""" + if rgb_image is None: + return + if self.rgb_image_handle is None: + 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: + """TODO: Document this method.""" + self.urdf_vis.update_cfg(joint_config) + + def update_ghost_robot_pose(self, joint_config: np.ndarray) -> None: + """TODO: Document this method.""" + if self.ghost_robot_urdf: + self.ghost_robot_urdf.update_cfg(joint_config) + + def update_ghost_robot_visibility(self, flag: bool) -> None: + """TODO: Document this method.""" + if self.ghost_robot_urdf: + self.ghost_robot_urdf.show_visual = flag + + def get_ghost_robot_visibility(self) -> bool: + """TODO: Document this method.""" + if self.ghost_robot_urdf: + return self.ghost_robot_urdf.show_visual + return False + + def set_ghost_robot_color(self, color: tuple[float, float, float, float]) -> None: + """TODO: Document this method.""" + if self.ghost_robot_urdf: + self.ghost_robot_urdf.mesh_color_override = color + + def update_controller_visualization(self, transform: np.ndarray | None) -> None: + """TODO: Document this method.""" + if self.controller_handle is None or transform is None: + return + pos = transform[:3, 3] + 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: + """TODO: Document this method.""" + 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: + """TODO: Document this method.""" + self.server.stop() diff --git a/examples/common/visualizer_gui.py b/examples/common/visualizer_gui.py new file mode 100644 index 0000000..7c5f249 --- /dev/null +++ b/examples/common/visualizer_gui.py @@ -0,0 +1,443 @@ +"""Module-level helpers for the robot visualizer.""" + +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: + """TODO: Document this method.""" + self.server = server + self._ema_timing = 0.001 + + # State Handles + self._timing_handle: Any = None + self._joint_angles_handle: Any = None + self._robot_status_handle: Any = None + self._teleop_status_handle: Any = None + self._controller_status_handle: Any = None + self._gripper_status_handle: Any = None + self._policy_status_handle: Any = None + + # Input Handles + self._position_weight_handle: Any = None + self._orientation_weight_handle: Any = None + self._frame_task_gain_handle: Any = None + self._lm_damping_handle: Any = None + self._damping_weight_handle: Any = None + self._solver_damping_value_handle: Any = None + self._posture_cost_handles: list[Any] = [] + + self._controller_min_cutoff_handle: Any = None + self._controller_beta_handle: Any = None + self._controller_d_cutoff_handle: Any = None + self._translation_scale_handle: Any = None + self._rotation_scale_handle: Any = None + + self._prediction_ratio_handle: Any = None + self._policy_execution_rate_handle: Any = None + self._robot_rate_handle: Any = None + self._execution_mode_dropdown: Any = None + + self._grip_value_handle: Any = None + self._trigger_value_handle: Any = None + + # Button Handles + self._enable_robot_handle: Any = None + self._disable_robot_handle: Any = None + self._emergency_stop_handle: Any = None + self._go_home_button: Any = None + self._toggle_robot_enabled_status_button: Any = None + self._run_policy_button: Any = None + self._start_policy_execution_button: Any = None + self._play_policy_button: Any = None + + self._slow_translation_scale_handle: Any = None + self._slow_rotation_scale_handle: Any = None + self._wrist_step_handle: Any = None + self._save_config_button: Any = None + + def add_advanced_tuning_controls( + self, initial_slow_t: float, initial_slow_r: float, initial_wrist_step: float + ) -> 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: + """TODO: Document this method.""" + return self._slow_translation_scale_handle.value + + def get_slow_rotation_scale(self) -> float: + """TODO: Document this method.""" + return self._slow_rotation_scale_handle.value + + def get_wrist_step_degrees(self) -> float: + """TODO: Document this method.""" + return self._wrist_step_handle.value + + def set_save_config_callback(self, cb: Callable[[], Any]) -> None: + """TODO: Document this method.""" + if self._save_config_button: + self._save_config_button.on_click(lambda _: cb()) + + def add_basic_controls(self) -> None: + """TODO: Document this method.""" + self._timing_handle = self.server.gui.add_number( + "IK Solve Time (ms)", 0.001, disabled=True + ) + self._joint_angles_handle = self.server.gui.add_text( + "Joint Angles", "Waiting..." + ) + + def add_robot_status_controls(self) -> None: + """TODO: Document this method.""" + self._robot_status_handle = self.server.gui.add_text( + "Robot Status", "Initializing..." + ) + + def add_teleop_controls(self) -> None: + """TODO: Document this method.""" + self._grip_value_handle = self.server.gui.add_number( + "Grip Value", 0.0, disabled=True + ) + 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: + """TODO: Document this method.""" + self._gripper_status_handle = self.server.gui.add_text( + "Gripper Status", "Open (0%)" + ) + + def add_homing_controls(self) -> None: + """TODO: Document this method.""" + self._go_home_button = self.server.gui.add_button("Go Home") + + def add_toggle_robot_enabled_status_button(self) -> None: + """TODO: Document this method.""" + self._toggle_robot_enabled_status_button = self.server.gui.add_button( + "Enable Robot" + ) + + def add_controller_filter_controls( + self, initial_min_cutoff: float, initial_beta: float, initial_d_cutoff: float + ) -> None: + """TODO: Document this method.""" + self._controller_min_cutoff_handle = self.server.gui.add_number( + "Controller Min Cutoff", initial_min_cutoff, min=0.01, max=10.0, step=0.01 + ) + 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: + """TODO: Document this method.""" + 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: + """TODO: Document this method.""" + self._position_weight_handle = self.server.gui.add_number( + "Position Weight", position_cost, min=0.0, max=10.0, step=0.1 + ) + 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: + """TODO: Document this method.""" + self._policy_status_handle = self.server.gui.add_text("Policy Status", "Ready") + self._prediction_ratio_handle = self.server.gui.add_number( + "Prediction Ratio", initial_prediction_ratio, min=0.0, max=1.0, step=0.01 + ) + 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: + """TODO: Document this method.""" + self._run_policy_button = self.server.gui.add_button("Run Policy (Preview)") + self._start_policy_execution_button = self.server.gui.add_button( + "Execute Policy (Run Preview)" + ) + self._play_policy_button = self.server.gui.add_button( + "Continuous Receding Horizon" + ) + + # --- UI Update Methods --- + def update_timing(self, solve_time_ms: float) -> None: + """TODO: Document this method.""" + if self._timing_handle: + self._ema_timing = 0.99 * self._ema_timing + 0.01 * solve_time_ms + self._timing_handle.value = self._ema_timing + + def update_robot_status(self, status: str) -> None: + """TODO: Document this method.""" + if self._robot_status_handle: + """TODO: Document this method.""" + self._robot_status_handle.value = status + + def update_teleop_status(self, active: bool) -> None: + """TODO: Document this method.""" + if self._teleop_status_handle: + """TODO: Document this method.""" + self._teleop_status_handle.value = ( + f"Teleop Status: {'Active' if active else 'Inactive'}" + ) + + def update_controller_status_display( + self, position: np.ndarray | None, connected: bool = True + ) -> None: + """TODO: Document this method.""" + if not self._controller_status_handle: + return + if connected and position is not None: + 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: + """TODO: Document this method.""" + 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: + """TODO: Document this method.""" + if self._policy_status_handle: + """TODO: Document this method.""" + self._policy_status_handle.value = status + + def update_toggle_robot_enabled_status(self, enabled: bool) -> None: + """TODO: Document this method.""" + if self._toggle_robot_enabled_status_button: + """TODO: Document this method.""" + self._toggle_robot_enabled_status_button.label = ( + "Disable Robot" if enabled else "Enable Robot" + ) + + def update_play_policy_button_status(self, active: bool) -> None: + """TODO: Document this method.""" + if self._play_policy_button: + """TODO: Document this method.""" + self._play_policy_button.label = ( + "Stop Continuous Horizon" if active else "Continuous Receding Horizon" + ) + + def update_joint_angles_display( + self, joint_config: np.ndarray, show_gripper: bool = False + ) -> None: + """TODO: Document this method.""" + if not self._joint_angles_handle: + return + lines = ["Joint Angles (rad):"] + 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: + """TODO: Document this method.""" + if self._grip_value_handle: + """TODO: Document this method.""" + self._grip_value_handle.value = value + + def set_trigger_value(self, value: float) -> None: + """TODO: Document this method.""" + if self._trigger_value_handle: + """TODO: Document this method.""" + self._trigger_value_handle.value = value + + # --- Getters --- + def get_controller_filter_params(self) -> tuple[float, float, float]: + """TODO: Document this method.""" + return ( + self._controller_min_cutoff_handle.value, + self._controller_beta_handle.value, + self._controller_d_cutoff_handle.value, + ) + + def get_translation_scale(self) -> float: + """TODO: Document this method.""" + return self._translation_scale_handle.value + + def get_rotation_scale(self) -> float: + """TODO: Document this method.""" + return self._rotation_scale_handle.value + + def get_prediction_ratio(self) -> float: + """TODO: Document this method.""" + return self._prediction_ratio_handle.value + + def get_policy_execution_rate(self) -> float: + """TODO: Document this method.""" + return self._policy_execution_rate_handle.value + + def get_robot_rate(self) -> float: + """TODO: Document this method.""" + return self._robot_rate_handle.value + + def get_execution_mode(self) -> str: + """TODO: Document this method.""" + return self._execution_mode_dropdown.value + + def get_pink_parameters(self) -> dict: + """TODO: Document this method.""" + return { + "position_cost": self._position_weight_handle.value, + "orientation_cost": self._orientation_weight_handle.value, + "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: + """TODO: Document this method.""" + if self._toggle_robot_enabled_status_button: + """TODO: Document this method.""" + self._toggle_robot_enabled_status_button.on_click(lambda _: cb()) + + def set_go_home_callback(self, cb: Callable[[], Any]) -> None: + """TODO: Document this method.""" + if self._go_home_button: + """TODO: Document this method.""" + self._go_home_button.on_click(lambda _: cb()) + + def set_run_policy_callback(self, cb: Callable[[], Any]) -> None: + """TODO: Document this method.""" + if self._run_policy_button: + """TODO: Document this method.""" + self._run_policy_button.on_click(lambda _: cb()) + + def set_start_policy_execution_callback(self, cb: Callable[[], Any]) -> None: + """TODO: Document this method.""" + if self._start_policy_execution_button: + """TODO: Document this method.""" + self._start_policy_execution_button.on_click(lambda _: cb()) + + def set_play_policy_callback(self, cb: Callable[[], Any]) -> None: + """TODO: Document this method.""" + if self._play_policy_button: + """TODO: Document this method.""" + self._play_policy_button.on_click(lambda _: cb()) + + def set_execution_mode_callback(self, cb: Callable[[], Any]) -> None: + """TODO: Document this method.""" + if self._execution_mode_dropdown: + """TODO: Document this method.""" + self._execution_mode_dropdown.on_update(lambda _: cb()) + + def set_run_policy_button_disabled(self, disabled: bool) -> None: + """TODO: Document this method.""" + if self._run_policy_button: + """TODO: Document this method.""" + self._run_policy_button.disabled = disabled + + def set_start_policy_execution_button_disabled(self, disabled: bool) -> None: + """TODO: Document this method.""" + if self._start_policy_execution_button: + """TODO: Document this method.""" + self._start_policy_execution_button.disabled = disabled + + def set_play_policy_button_disabled(self, disabled: bool) -> None: + """TODO: Document this method.""" + if self._play_policy_button: + """TODO: Document this method.""" + self._play_policy_button.disabled = disabled diff --git a/examples/ik_conf/default.yaml b/examples/ik_conf/default.yaml new file mode 100644 index 0000000..c538ad7 --- /dev/null +++ b/examples/ik_conf/default.yaml @@ -0,0 +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 # 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 + + # 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/piper_controller.py b/piper_controller.py index e979bcc..bc87a60 100644 --- a/piper_controller.py +++ b/piper_controller.py @@ -91,7 +91,9 @@ def __init__( # Gripper range in degrees (for internal SDK communication) self.GRIPPER_DEGREES_MIN = -10.00 - self.GRIPPER_DEGREES_MAX = 30.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 @@ -223,15 +225,31 @@ 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: + """Set the current robot control mode and propagate the configuration to the hardware.""" with self.state_lock: old_mode = self._control_mode self._control_mode = mode + + # 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}") @@ -491,7 +509,9 @@ def control_loop(self) -> None: if now - last_reenable_time >= reenable_interval: if not self.piper.EnablePiper(): if self.debug_mode: - print("Control loop: EnablePiper re-enable returned False") + print( + "Control loop: EnablePiper re-enable returned False" + ) last_reenable_time = now # Get current control mode