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/environment.yaml b/environment.yaml
index 2fc42d5..b6f46f6 100644
--- a/environment.yaml
+++ b/environment.yaml
@@ -7,7 +7,7 @@ dependencies:
- pip:
- piper-sdk==0.6.1
- pin
- - pin-pink
+ - pin-pink==3.5.0
- qpsolvers[quadprog]
- yourdfpy
- pure-python-adb
diff --git a/example-agilex-dictionary.txt b/example-agilex-dictionary.txt
index 25fbae6..5a0a7f0 100644
--- a/example-agilex-dictionary.txt
+++ b/example-agilex-dictionary.txt
@@ -49,3 +49,11 @@ Viser
wxyz
xyzw
yourdfpy
+Lerobot
+readchar
+ecodes
+evdev
+keystate
+pynput
+ungrab
+
diff --git a/examples/1_tune_teleop_params.py b/examples/1_tune_teleop_params.py
index 47f748a..44ae16b 100644
--- a/examples/1_tune_teleop_params.py
+++ b/examples/1_tune_teleop_params.py
@@ -1,11 +1,11 @@
#!/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
@@ -13,384 +13,259 @@
import threading
import time
import traceback
+import yaml
+import os
from pathlib import Path
-
import numpy as np
-# Add parent directory to path to import pink_ik_solver and piper_controller
+# Add parent directory to path to import local modules
sys.path.insert(0, str(Path(__file__).parent.parent))
+# Only import the fixed hardware constants that are left in configs.py
from common.configs import (
- CAMERA_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,
- NEUTRAL_JOINT_ANGLES,
- ORIENTATION_COST,
- POSITION_COST,
- POSTURE_COST_VECTOR,
- ROBOT_RATE,
- ROTATION_SCALE,
- SOLVER_DAMPING_VALUE,
- SOLVER_NAME,
- TRANSLATION_SCALE,
- URDF_PATH,
- VISUALIZATION_RATE,
+ CAMERA_NAMES, META_QUEST_AXIS_MASK, URDF_PATH, VISUALIZATION_RATE
)
-from common.data_manager import DataManager, RobotActivityState
+from common.data_manager import RobotActivityState
+from common.system_bootstrap import bootstrap_robot_system
+from common.shared_actions import toggle_robot_enabled, move_robot_home
from common.robot_visualizer import RobotVisualizer
-from common.threads.camera import camera_thread
-from common.threads.ik_solver import ik_solver_thread
-from common.threads.joint_state import joint_state_thread
from common.threads.quest_reader import quest_reader_thread
+from common.config_parser import load_ik_config
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:
- # 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")
-
-
-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,
- 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)
-
-# 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)
-
-# Start quest reader thread
-print("\n๐ฎ Starting quest reader thread...")
-quest_thread = threading.Thread(
- target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True
-)
-quest_thread.start()
-
-# set initial configuration to current joint angles
-current_joint_angles = data_manager.get_current_joint_angles()
-if current_joint_angles is not None:
- initial_joint_angles = np.radians(current_joint_angles)
-else:
- initial_joint_angles = np.radians(NEUTRAL_JOINT_ANGLES)
-
-# Create Pink IK solver
-print("\n๐ง Creating Pink IK solver...")
-ik_solver = PinkIKSolver(
- urdf_path=URDF_PATH,
- end_effector_frame=GRIPPER_FRAME_NAME,
- solver_name=SOLVER_NAME,
- position_cost=POSITION_COST,
- orientation_cost=ORIENTATION_COST,
- frame_task_gain=FRAME_TASK_GAIN,
- lm_damping=LM_DAMPING,
- damping_cost=DAMPING_COST,
- solver_damping_value=SOLVER_DAMPING_VALUE,
- integration_time_step=1 / IK_SOLVER_RATE,
- initial_configuration=initial_joint_angles,
- posture_cost_vector=np.array(POSTURE_COST_VECTOR),
-)
-
-# Start IK solver thread
-print("\n๐งฎ Starting IK solver thread...")
-ik_thread = threading.Thread(
- target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True
-)
-ik_thread.start()
-
-# Start camera thread (if RealSense is available)
-print("\n๐ท Starting camera thread...")
-camera_thread_obj = threading.Thread(
- target=camera_thread, args=(data_manager,), daemon=True
-)
-camera_thread_obj.start()
-
-
-# Set up visualizer
-print("\n๐ฅ๏ธ Starting visualization...")
-visualizer = RobotVisualizer(urdf_path=URDF_PATH)
-visualizer.add_basic_controls()
-visualizer.add_teleop_controls()
-visualizer.add_gripper_status_controls()
-visualizer.add_homing_controls()
-visualizer.add_toggle_robot_enabled_status_button()
-visualizer.add_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()
-visualizer.add_target_frame_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)
+# ---------------------------------------------------------------------------
+# Dynamic Action Callbacks
+# ---------------------------------------------------------------------------
+
+def _step_wrist_joint(data_manager, robot_controller, visualizer, direction: int) -> None:
+ """Apply a relative manual step to the wrist joint using the dynamically tuned step degree."""
+ data_manager.set_teleop_state(False, None, None)
+ robot_controller.set_control_mode(PiperController.ControlMode.JOINT_SPACE)
+
+ target_joint_angles = robot_controller.get_target_joint_angles()
+ current_joint_angles = data_manager.get_current_joint_angles()
+
+ base_wrist = float(current_joint_angles[-1]) if current_joint_angles is not None else float(target_joint_angles[-1])
+
+ # Read the tuned value directly from the UI and apply direction multiplier (1 or -1)
+ delta_degrees = visualizer.get_wrist_step_degrees() * direction
+ target_joint_angles[-1] = base_wrist + delta_degrees
+
+ robot_controller.set_target_joint_angles(target_joint_angles)
+ data_manager.set_target_joint_angles(target_joint_angles)
+
+
+def toggle_slow_scaling(data_manager, visualizer):
+ """Toggle the movement scaling mode between standard and dynamically-tuned precision."""
+ enabled = data_manager.toggle_slow_scaling_mode_enabled()
+ if enabled:
+ print(f"๐ข Slow scaling enabled (trans={visualizer.get_slow_translation_scale():.3f}, rot={visualizer.get_slow_rotation_scale():.3f})")
else:
- print("โ ๏ธ Cannot home: robot not enabled")
+ print(f"๐ Slow scaling disabled (trans={visualizer.get_translation_scale():.3f}, rot={visualizer.get_rotation_scale():.3f})")
-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. Release grip to stop")
-print(" 7. 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 (module-level variables used by IK thread)
- TRANSLATION_SCALE = visualizer.get_translation_scale()
- ROTATION_SCALE = visualizer.get_rotation_scale()
-
- # Update Pink parameters (GUI controls)
+def save_config_to_yaml(visualizer, filepath="ik_conf/tuned_teleop_configs.yaml") -> None:
+ """Extracts all current UI slider values and saves them to a YAML file."""
+ try:
+ # Ensure the directory exists before saving
+ os.makedirs(os.path.dirname(filepath), exist_ok=True)
+
pink_params = visualizer.get_pink_parameters()
- 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)}")
+ 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
)
- 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()
-
- # 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:
+ except (Exception, SystemExit) as e:
+ print("\n" + "!" * 60)
+ print("โ FAILED TO ACCESS META QUEST")
+ print("!" * 60)
+ print("The headset is plugged in, but ADB debugging permissions are missing.")
+ print("\nPLEASE FOLLOW THESE STEPS:")
+ print(" 1. Put the Meta Quest headset on your head.")
+ print(" 2. Look for a notification in your menu that says 'USB Detected'.")
+ print(" 3. Click on that notification and select 'Allow' to grant data access.")
+ print(" 4. Rerun this script.")
+ print("!" * 60 + "\n")
+
+ data_manager.request_shutdown()
+ for t in active_threads:
+ t.join(timeout=1.0)
+ robot_controller.cleanup()
+ sys.exit(1)
+
+ # 3. Initialize Visualizer 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()
+ r_scale = visualizer.get_slow_rotation_scale() if data_manager.get_slow_scaling_mode_enabled() else visualizer.get_rotation_scale()
+ data_manager.set_teleop_scaling(t_scale, r_scale)
+
+ # Retrieve State Data
+ controller_transform, grip_value, trigger_value = data_manager.get_controller_data()
+ teleop_active = data_manager.get_teleop_active()
+ state = data_manager.get_robot_activity_state()
+ current_joints = data_manager.get_current_joint_angles()
+ target_joints = data_manager.get_target_joint_angles()
+ rgb_image = data_manager.get_rgb_image(CAMERA_NAMES[0])
+
+ # Update GUI Displays
+ visualizer.set_grip_value(grip_value)
+ visualizer.set_trigger_value(trigger_value)
+ visualizer.update_timing(data_manager.get_ik_solve_time_ms())
+ visualizer.update_controller_visualization(controller_transform)
visualizer.update_controller_status_display(
- controller_transform[:3, 3], connected=True
+ controller_transform[:3, 3] if controller_transform is not None else None,
+ connected=controller_transform is not None
)
- else:
- visualizer.update_controller_status_display(None, connected=False)
-
- # Update teleop status display
- visualizer.update_teleop_status(teleop_active)
-
- # Update target/goal visualization
- visualizer.update_target_visualization(target_pose)
-
- # Update main robot visualization from CURRENT joint angles (DataManager uses degrees, Viser expects radians)
- if current_joint_angles is not None:
- current_joint_rad = np.radians(current_joint_angles)
- visualizer.update_robot_pose(current_joint_rad)
- visualizer.update_joint_angles_display(current_joint_rad)
-
- # Update ghost robot to show TARGET joint angles when available (also in radians)
- if (
- target_joint_angles is not None
- and robot_activity_state == RobotActivityState.ENABLED
- ):
- target_joint_rad = np.radians(target_joint_angles)
- visualizer.update_ghost_robot_visibility(True)
- visualizer.update_ghost_robot_pose(target_joint_rad)
- else:
- # Hide ghost robot when no valid target is available or robot not enabled
- visualizer.update_ghost_robot_visibility(False)
-
- # Update robot status - simple Enabled/Homing/Disabled
- if robot_activity_state == RobotActivityState.ENABLED:
- visualizer.update_robot_status("Robot Status: Enabled")
- elif robot_activity_state == RobotActivityState.HOMING:
- visualizer.update_robot_status("Robot Status: Homing")
- else: # DISABLED
- visualizer.update_robot_status("Robot Status: Disabled")
-
- # Update gripper status
- visualizer.update_gripper_status(
- trigger_value,
- robot_enabled=(robot_activity_state == RobotActivityState.ENABLED),
- )
-
- # Sleep to maintain visualization rate
- elapsed = time.time() - iteration_start
- sleep_time = dt - elapsed
- if sleep_time > 0:
- time.sleep(sleep_time)
-
-except KeyboardInterrupt:
- print("\n\n๐ Interrupt received - shutting down gracefully...")
-except Exception as e:
- print(f"\nโ Demo error: {e}")
- import traceback
-
- traceback.print_exc()
-
-# Cleanup (outside try/except so it always runs)
-print("\n๐งน Cleaning up...")
-
-data_manager.request_shutdown()
-data_manager.set_robot_activity_state(RobotActivityState.DISABLED)
-quest_thread.join()
-quest_reader.stop()
-ik_thread.join()
-joint_state_thread_obj.join()
-robot_controller.cleanup()
-visualizer.stop()
-
-print("\n๐ Demo stopped.")
+ visualizer.update_teleop_status(teleop_active)
+ visualizer.update_target_visualization(data_manager.get_target_pose())
+ visualizer.update_rgb_image(rgb_image)
+
+ # Render Actual Robot
+ if current_joints is not None:
+ rad_joints = np.radians(current_joints)
+ visualizer.update_robot_pose(rad_joints)
+ visualizer.update_joint_angles_display(rad_joints)
+
+ # Render Ghost Robot Target
+ if target_joints is not None and state == RobotActivityState.ENABLED:
+ visualizer.update_ghost_robot_visibility(True)
+ visualizer.update_ghost_robot_pose(np.radians(target_joints))
+ else:
+ visualizer.update_ghost_robot_visibility(False)
+
+ visualizer.update_robot_status(f"Robot Status: {state.value.capitalize()}")
+ visualizer.update_gripper_status(trigger_value, robot_enabled=(state == RobotActivityState.ENABLED))
+
+ # Maintain 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.")
\ No newline at end of file
diff --git a/examples/2_collect_teleop_data_with_neuracore.py b/examples/2_collect_teleop_data_with_neuracore.py
index fedff4e..11ac518 100644
--- a/examples/2_collect_teleop_data_with_neuracore.py
+++ b/examples/2_collect_teleop_data_with_neuracore.py
@@ -1,10 +1,27 @@
#!/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
@@ -14,321 +31,177 @@
import traceback
from pathlib import Path
-import neuracore as nc
import numpy as np
+import neuracore as nc
-# Add parent directory to path to import pink_ik_solver and piper_controller
+# ---------------------------------------------------------------------------
+# 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,
- GRIPPER_LOGGING_NAME,
- IK_SOLVER_RATE,
- JOINT_NAMES,
- JOINT_STATE_STREAMING_RATE,
- LM_DAMPING,
- NEUTRAL_JOINT_ANGLES,
- ORIENTATION_COST,
- POSITION_COST,
- POSTURE_COST_VECTOR,
- ROBOT_RATE,
- SOLVER_DAMPING_VALUE,
- SOLVER_NAME,
- URDF_PATH,
+from common.configs import META_QUEST_AXIS_MASK, URDF_PATH
+from common.config_parser import load_ik_config
+from common.data_manager import RobotActivityState, DataManager
+from common.system_bootstrap import bootstrap_robot_system
+from common.shared_actions import (
+ toggle_robot_enabled, move_robot_home,
+ toggle_recording, neuracore_logging_callback
)
-from common.data_manager import DataManager, RobotActivityState
-from common.threads.camera import camera_thread
-from common.threads.ik_solver import ik_solver_thread
-from common.threads.joint_state import joint_state_thread
from common.threads.quest_reader import quest_reader_thread
from meta_quest_teleop.reader import MetaQuestReader
-
-from pink_ik_solver import PinkIKSolver
from piper_controller import PiperController
+# ---------------------------------------------------------------------------
+# Helper Functions
+# ---------------------------------------------------------------------------
-def log_to_neuracore_on_change_callback(
- name: str, value: float, timestamp: float
-) -> None:
- """Log data to queue on change callback."""
- # Call appropriate Neuracore logging function
- try:
- if name == "log_joint_positions":
- data_value = np.radians(value)
- data_dict = {
- joint_name: angle for joint_name, angle in zip(JOINT_NAMES, data_value)
- }
- nc.log_joint_positions(data_dict, timestamp=timestamp)
- elif name == "log_joint_target_positions":
- data_value = np.radians(value)
- data_dict = {
- joint_name: angle for joint_name, angle in zip(JOINT_NAMES, data_value)
- }
- nc.log_joint_target_positions(data_dict, timestamp=timestamp)
- elif name == "log_parallel_gripper_open_amounts":
- data_dict = {GRIPPER_LOGGING_NAME: value}
- nc.log_parallel_gripper_open_amounts(data_dict, timestamp=timestamp)
- elif name == "log_parallel_gripper_target_open_amounts":
- data_dict = {GRIPPER_LOGGING_NAME: value}
- nc.log_parallel_gripper_target_open_amounts(data_dict, timestamp=timestamp)
- elif name == "log_rgb":
- camera_name = "rgb"
- image_array = value
- 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 _step_wrist_joint(data_manager: DataManager, robot_controller: PiperController, delta_degrees: float) -> None:
+ """
+ Applies a relative step adjustment to the robot's wrist joint target angle.
+ """
+ data_manager.set_teleop_state(False, None, None)
+ robot_controller.set_control_mode(PiperController.ControlMode.JOINT_SPACE)
-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()
+ target_joint_angles = robot_controller.get_target_joint_angles()
+ current_joint_angles = data_manager.get_current_joint_angles()
+
+ # Fallback to target angles if hardware telemetry is temporarily unavailable
+ base_wrist = float(current_joint_angles[-1]) if current_joint_angles is not None else float(target_joint_angles[-1])
+
+ target_joint_angles[-1] = base_wrist + delta_degrees
+ robot_controller.set_target_joint_angles(target_joint_angles)
+ data_manager.set_target_joint_angles(target_joint_angles)
+
+
+def toggle_slow_scaling(data_manager: DataManager, tele_p: dict) -> None:
+ """
+ Toggles the teleoperation sensitivity between standard and slow (precision) scaling.
+ """
+ enabled = data_manager.toggle_slow_scaling_mode_enabled()
+ if enabled:
+ data_manager.set_teleop_scaling(
+ tele_p.get("slow_translation_scale", 0.6),
+ tele_p.get("slow_rotation_scale", 0.6)
+ )
+ print("๐ข Slow scaling (precision mode) enabled")
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()
+ 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"
- )
- parser.add_argument(
- "--ip-address",
- type=str,
- default=None,
- help="IP address of Meta Quest device (optional, defaults to None for auto-discovery)",
- )
- parser.add_argument(
- "--dataset-name",
- type=str,
- default=None,
- help="Name for the dataset (optional, defaults to auto-generated timestamp-based name)",
- )
+
+ parser = argparse.ArgumentParser(description="Teleop with Neuracore Data Collection")
+ parser.add_argument("--ip-address", type=str, default=None, help="IP address of the Meta Quest headset.")
+ parser.add_argument("--dataset-name", type=str, default=None, help="Override the auto-generated dataset name.")
+ parser.add_argument("--ik-config", type=str, default="ik_conf/default.yaml", help="Path to IK/teleop YAML config.")
args = parser.parse_args()
- print("=" * 60)
- print("PIPER ROBOT TELEOPERATION - REAL ROBOT CONTROL")
- print("=" * 60)
- print("Thread frequencies:")
- print(f" ๐ฎ Quest Controller: {CONTROLLER_DATA_RATE} Hz")
- print(f" ๐งฎ IK Solver: {IK_SOLVER_RATE} Hz")
- print(f" ๐ค Robot Controller: {ROBOT_RATE} Hz")
- print(f" ๐ธ Camera Frame: {CAMERA_FRAME_STREAMING_RATE} Hz")
- print(f" ๐ Joint State: {JOINT_STATE_STREAMING_RATE} Hz")
+ print("=" * 60 + "\nPIPER TELEOP DATA COLLECTION\n" + "=" * 60)
+
+ # 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)
- # Connect to Neuracore
+ # ---------------------------------------------------------
+ # 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,
- )
-
- # Create dataset
- dataset_name = (
- args.dataset_name or f"piper-teleop-data-{time.strftime('%Y-%m-%d-%H-%M-%S')}"
- )
- print(f"\n๐ง Creating dataset {dataset_name}...")
- nc.create_dataset(
- name=dataset_name,
- description="Teleop data collection for Piper robot",
- )
-
- # Initialize shared state
- data_manager = DataManager()
- data_manager.set_on_change_callback(log_to_neuracore_on_change_callback)
- data_manager.set_controller_filter_params(
- CONTROLLER_MIN_CUTOFF,
- CONTROLLER_BETA,
- CONTROLLER_D_CUTOFF,
+ nc.connect_robot(robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False)
+
+ # Auto-generate a timestamped dataset name if one was not provided via CLI
+ dataset_name = args.dataset_name or f"piper-teleop-data-{time.strftime('%Y-%m-%d-%H-%M-%S')}"
+ nc.create_dataset(name=dataset_name, description="Teleop data collection for Piper robot")
+
+ # ---------------------------------------------------------
+ # 2. Hardware & Subsystem Bootstrapping
+ # ---------------------------------------------------------
+ data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system(
+ config, start_ik=True, start_camera=True
)
+
+ # Bind the unified Neuracore logger so state changes automatically push to the cloud
+ data_manager.set_on_change_callback(neuracore_logging_callback)
- # 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
+ # ---------------------------------------------------------
+ # 3. Meta Quest Initialization & Error Handling
+ # ---------------------------------------------------------
print("\n๐ฎ Initializing Meta Quest reader...")
- quest_reader = MetaQuestReader(
- ip_address=args.ip_address,
- port=5555,
- run=True,
- )
-
- # 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)
-
- # 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
- )
+ try:
+ quest_reader = MetaQuestReader(
+ ip_address=args.ip_address, port=5555, run=True, axis_mask=META_QUEST_AXIS_MASK
+ )
+ except (Exception, SystemExit) as e:
+ print("\n" + "!" * 60)
+ print("โ FAILED TO ACCESS META QUEST")
+ print("!" * 60)
+ print("The headset is plugged in, but ADB debugging permissions are missing.")
+ print("\nPLEASE FOLLOW THESE STEPS:")
+ print(" 1. Put the Meta Quest headset on your head.")
+ print(" 2. Look for a notification in your menu that says 'USB Detected'.")
+ print(" 3. Click on that notification and select 'Allow' to grant data access.")
+ print(" 4. Rerun this script.")
+ print("!" * 60 + "\n")
+
+ data_manager.request_shutdown()
+ for t in active_threads:
+ t.join(timeout=1.0)
+ robot_controller.cleanup()
+ nc.logout()
+ sys.exit(1)
+
+ # ---------------------------------------------------------
+ # 4. Input Binding & Background Threading
+ # ---------------------------------------------------------
+ quest_reader.on("button_a_pressed", lambda: toggle_robot_enabled(data_manager, robot_controller))
+ quest_reader.on("button_b_pressed", lambda: move_robot_home(data_manager, robot_controller))
+ quest_reader.on("button_rj_pressed", lambda: toggle_recording(play_audio=True))
+ quest_reader.on("button_y_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, wrist_step))
+ quest_reader.on("button_x_pressed", lambda: _step_wrist_joint(data_manager, robot_controller, -wrist_step))
+ quest_reader.on("button_lj_pressed", lambda: toggle_slow_scaling(data_manager, tele_p))
+
+ quest_thread = threading.Thread(target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True)
quest_thread.start()
+ active_threads.append(quest_thread)
- # set initial configuration to current joint angles
- current_joint_angles = data_manager.get_current_joint_angles()
- if current_joint_angles is not None:
- initial_joint_angles = np.radians(current_joint_angles)
- else:
- initial_joint_angles = np.radians(NEUTRAL_JOINT_ANGLES)
-
- # Create Pink IK solver
- print("\n๐ง Creating Pink IK solver...")
- ik_solver = PinkIKSolver(
- urdf_path=URDF_PATH,
- end_effector_frame=GRIPPER_FRAME_NAME,
- solver_name=SOLVER_NAME,
- position_cost=POSITION_COST,
- orientation_cost=ORIENTATION_COST,
- frame_task_gain=FRAME_TASK_GAIN,
- lm_damping=LM_DAMPING,
- damping_cost=DAMPING_COST,
- solver_damping_value=SOLVER_DAMPING_VALUE,
- integration_time_step=1 / IK_SOLVER_RATE,
- initial_configuration=initial_joint_angles,
- posture_cost_vector=np.array(POSTURE_COST_VECTOR),
- )
-
- # Start IK solver thread
- print("\n๐งฎ Starting IK solver thread...")
- ik_thread = threading.Thread(
- target=ik_solver_thread, args=(data_manager, ik_solver), daemon=True
- )
- ik_thread.start()
-
- # Start 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()
-
- 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. Release grip to stop")
- print("โ ๏ธ Press Ctrl+C to exit")
- print()
+ print("\n๐ Ready! Use Meta Quest controllers. Press Right Joystick to Record. Press Ctrl+C to exit.\n")
+ # ---------------------------------------------------------
+ # 5. Main Daemon Loop
+ # ---------------------------------------------------------
try:
- while True:
+ while not data_manager.is_shutdown_requested():
time.sleep(1)
+
except KeyboardInterrupt:
print("\n๐ Interrupt received - shutting down gracefully...")
+ data_manager.request_shutdown()
except Exception as e:
- print(f"\nโ Demo error. Exception: {e}")
- print("Traceback:")
- traceback.print_exc()
- # Cleanup
- print("\n๐งน Cleaning up...")
-
- # Cancel recording if active
+ print(f"\nโ Unhandled Demo error: {e}")
+ data_manager.request_shutdown()
+
+ # ---------------------------------------------------------
+ # 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
- nc.logout()
- data_manager.request_shutdown()
+ nc.cancel_recording()
+
data_manager.set_robot_activity_state(RobotActivityState.DISABLED)
- quest_thread.join()
quest_reader.stop()
- ik_thread.join()
- 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.")
\ No newline at end of file
diff --git a/examples/3_replay_neuracore_episodes.py b/examples/3_replay_neuracore_episodes.py
index fc06f26..3a5cdc3 100644
--- a/examples/3_replay_neuracore_episodes.py
+++ b/examples/3_replay_neuracore_episodes.py
@@ -1,4 +1,37 @@
-"""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,188 +42,204 @@
import cv2
import neuracore as nc
import numpy as np
-from common.configs import (
- GRIPPER_LOGGING_NAME,
- JOINT_NAMES,
- NEUTRAL_JOINT_ANGLES,
- ROBOT_RATE,
-)
-from neuracore_types import DataType, RobotDataSpec, 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:
+ """
+ Commands the robot to its home position and blocks 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",
+ can_interface="can0",
robot_rate=ROBOT_RATE,
control_mode=PiperController.ControlMode.JOINT_SPACE,
- neutral_joint_angles=NEUTRAL_JOINT_ANGLES,
+ neutral_joint_angles=NEUTRAL_JOINT_ANGLES,
debug_mode=False,
)
- # 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)
-
- # Build robot_data_spec for synchronization
- print("\n๐ Building robot data spec for synchronization...")
- data_types_to_synchronize = [
- DataType.JOINT_POSITIONS,
- DataType.JOINT_TARGET_POSITIONS,
- DataType.RGB_IMAGES,
- DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS,
- DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS,
- ]
- robot_data_spec: RobotDataSpec = {}
- robot_ids_dataset = dataset.robot_ids
- for robot_id in robot_ids_dataset:
- data_type_to_names = dataset.get_full_data_spec(robot_id)
- robot_data_spec[robot_id] = {
- data_type: data_type_to_names[data_type]
- for data_type in data_types_to_synchronize
- }
-
- # Synchronize dataset
- print("\n๐ Synchronizing dataset...")
- synced_dataset = dataset.synchronize(
- frequency=args.frequency,
- robot_data_spec=robot_data_spec,
+ input_mods = [DataType.JOINT_POSITIONS, DataType.RGB_IMAGES, DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS]
+ output_mods = [DataType.JOINT_TARGET_POSITIONS, DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS]
+
+ synced_dataset = load_and_sync_dataset(
+ args.dataset_name,
+ args.frequency,
+ input_mods,
+ output_mods
)
- # 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]
+
+ if len(episode) == 0:
+ print(f"โ ๏ธ Warning: Episode {episode_idx} is empty. Skipping...")
+ continue
- robot_controller.move_to_home()
- seconds_to_wait = 10
- while not robot_controller.is_robot_homed():
- time.sleep(1)
- seconds_to_wait -= 1
- if seconds_to_wait <= 0:
- break
- print(
- f"๐ Waiting for robot to reach home position... {seconds_to_wait} seconds remaining."
- )
- if robot_controller.is_robot_homed():
- print("โ Robot is at home position.")
- else:
- print("โ Robot did not reach home position within 10 seconds.")
- print(
- f"๐ Current joint angles: {robot_controller.get_current_joint_angles()}"
- )
- print(f"๐ Home joint angles: {robot_controller.HOME_JOINT_ANGLES}")
-
- print(f"\n{'='*60}")
- print(f"๐ฌ Playing Episode {episode_idx} / {len(synced_dataset) - 1}")
- print(f"{'='*60}")
+ 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)
+
+ 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()})
- # 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_LOGGING_NAME in gripper_data:
- gripper_value = gripper_data[GRIPPER_LOGGING_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)
+ 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_gripper_open_value(
- parallel_gripper_open_amounts[index]
- )
-
- # Display camera frames (dataset stores RGB; OpenCV expects BGR)
- if index < len(rgb_frames_per_step):
- for camera_name, frame_rgb in rgb_frames_per_step[index].items():
- arr = np.asarray(frame_rgb, dtype=np.uint8)
- frame_bgr = cv2.cvtColor(arr, cv2.COLOR_RGB2BGR)
- cv2.imshow(f"Replay: {camera_name}", frame_bgr)
+
+ robot_controller.set_target_joint_angles(joint_positions[idx])
+ robot_controller.set_gripper_open_value(parallel_gripper_open_amounts[idx])
+
+ if idx < len(rgb_frames_per_step):
+ for cam_name, frame_rgb in rgb_frames_per_step[idx].items():
+ 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.")
\ No newline at end of file
diff --git a/examples/4_rollout_neuracore_policy.py b/examples/4_rollout_neuracore_policy.py
index 3841ef9..2c089e0 100644
--- a/examples/4_rollout_neuracore_policy.py
+++ b/examples/4_rollout_neuracore_policy.py
@@ -1,9 +1,19 @@
#!/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,719 +24,84 @@
from pathlib import Path
import neuracore as nc
-import numpy as np
-from neuracore_types import (
- BatchedJointData,
- BatchedParallelGripperOpenAmountData,
- DataType,
-)
-# 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.configs import (
- CAMERA_FRAME_STREAMING_RATE,
- CAMERA_LOGGING_NAME,
- CONTROLLER_BETA,
- CONTROLLER_D_CUTOFF,
- CONTROLLER_DATA_RATE,
- CONTROLLER_MIN_CUTOFF,
- DAMPING_COST,
- FRAME_TASK_GAIN,
- GRIPPER_FRAME_NAME,
- GRIPPER_LOGGING_NAME,
- IK_SOLVER_RATE,
- JOINT_NAMES,
- JOINT_STATE_STREAMING_RATE,
- LM_DAMPING,
- MAX_ACTION_ERROR_THRESHOLD,
- MAX_SAFETY_THRESHOLD,
- NEUTRAL_JOINT_ANGLES,
- ORIENTATION_COST,
- POLICY_EXECUTION_RATE,
- POSITION_COST,
- POSTURE_COST_VECTOR,
- PREDICTION_HORIZON_EXECUTION_RATIO,
- ROBOT_RATE,
- SOLVER_DAMPING_VALUE,
- SOLVER_NAME,
- TARGETING_POSE_TIME_THRESHOLD,
- URDF_PATH,
- VISUALIZATION_RATE,
+ CAMERA_NAMES, GRIPPER_NAME, URDF_PATH,
+ PREDICTION_HORIZON_EXECUTION_RATIO, POLICY_EXECUTION_RATE, ROBOT_RATE,
)
-from common.data_manager import DataManager, RobotActivityState
+from neuracore_types import DataType, EmbodimentDescription
+
+from common.data_manager import RobotActivityState
from common.policy_state import PolicyState
+from common.policy_helpers import (
+ embodiment_names_ordered,
+ get_policy_embodiments,
+ print_policy_embodiments
+)
+from common.config_parser import load_ik_config
+from common.system_bootstrap import bootstrap_robot_system
+from common.shared_actions import toggle_robot_enabled, move_robot_home
from common.robot_visualizer import RobotVisualizer
-from common.threads.camera import camera_thread
-from common.threads.ik_solver import ik_solver_thread
-from common.threads.joint_state import joint_state_thread
-from common.threads.quest_reader import quest_reader_thread
-from meta_quest_teleop.reader import MetaQuestReader
-
-from pink_ik_solver import PinkIKSolver
-from piper_controller import PiperController
-
-
-def convert_predictions_to_horizon_dict(predictions: dict) -> dict[str, list[float]]:
- """Convert predictions dict to horizon dict format."""
- horizon: dict[str, list[float]] = {}
-
- # Extract joint target positions
- if DataType.JOINT_TARGET_POSITIONS in predictions:
- joint_data = predictions[DataType.JOINT_TARGET_POSITIONS]
- for joint_name in JOINT_NAMES:
- if joint_name in joint_data:
- batched = joint_data[joint_name]
- if isinstance(batched, BatchedJointData):
- # Extract values: (B, T, 1) -> list[float], taking B=0
- values = batched.value[0, :, 0].cpu().numpy().tolist()
- horizon[joint_name] = values
-
- # Extract gripper open amounts
- if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in predictions:
- gripper_data = predictions[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS]
- if GRIPPER_LOGGING_NAME in gripper_data:
- batched = gripper_data[GRIPPER_LOGGING_NAME]
- if isinstance(batched, BatchedParallelGripperOpenAmountData):
- # Extract values: (B, T, 1) -> list[float], taking B=0
- values = batched.open_amount[0, :, 0].cpu().numpy().tolist()
- horizon[GRIPPER_LOGGING_NAME] = values
-
- return horizon
-
-
-def toggle_robot_enabled_status(
- data_manager: DataManager,
- robot_controller: PiperController,
- visualizer: RobotVisualizer,
-) -> None:
- """Handle Button A press to toggle robot enable/disable state."""
- robot_activity_state = data_manager.get_robot_activity_state()
- if robot_activity_state == RobotActivityState.ENABLED:
- # Disable robot
- data_manager.set_robot_activity_state(RobotActivityState.DISABLED)
- robot_controller.graceful_stop()
- # Reset teleop state when disabling robot
- data_manager.set_teleop_state(False, None, None)
- visualizer.update_toggle_robot_enabled_status(False)
- print("โ ๐ด Robot disabled (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,
- model_input_order: dict[DataType, list[str]],
-) -> bool:
- """Handle Run Policy button press to capture state and get policy prediction."""
- print("Running policy...")
-
- # Get available data from data_manager (only log what the model expects)
- current_joint_angles = None
- gripper_open_value = None
- rgb_image = None
-
- # Only log data types that are in model_input_order
- if DataType.JOINT_POSITIONS in model_input_order:
- current_joint_angles = data_manager.get_current_joint_angles()
- if current_joint_angles is not None:
- joint_angles_rad = np.radians(current_joint_angles)
- joint_positions_dict = {
- joint_name: angle
- for joint_name, angle in zip(JOINT_NAMES, joint_angles_rad)
- }
- nc.log_joint_positions(joint_positions_dict)
- print(" โ Logged joint positions")
- else:
- print(" โ ๏ธ No current joint angles available")
-
- if DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS in model_input_order:
- gripper_open_value = data_manager.get_current_gripper_open_value()
- if gripper_open_value is not None:
- nc.log_parallel_gripper_open_amount(
- GRIPPER_LOGGING_NAME, gripper_open_value
- )
- print(" โ Logged gripper open amount")
- else:
- print(" โ ๏ธ No gripper open value available")
-
- if DataType.RGB_IMAGES in model_input_order:
- rgb_image = data_manager.get_rgb_image()
- if rgb_image is not None:
- nc.log_rgb(CAMERA_LOGGING_NAME, rgb_image)
- print(" โ Logged RGB image")
- else:
- print(" โ ๏ธ No RGB image available")
-
- # Check if we have at least some data to run the policy
- if (
- current_joint_angles is None
- and gripper_open_value is None
- and rgb_image is None
- ):
- print("โ No data available to run policy")
- return False
-
- # Get policy prediction
- try:
- start_time = time.time()
- predictions = policy.predict(timeout=5)
- prediction_horizon = convert_predictions_to_horizon_dict(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,
- model_input_order: dict[DataType, list[str]],
-) -> None:
- """Handle Run and Execute Policy button press to capture state, get policy prediction, and immediately execute it."""
- print("Run and Execute Policy for one prediction horizon")
- run_policy(data_manager, policy, policy_state, visualizer, model_input_order)
- 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,
- model_input_order: dict[DataType, list[str]],
-) -> 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, model_input_order
- )
- 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,
- model_input_order: dict[DataType, list[str]],
-) -> 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
- ):
- # Get previous action from horizon
- if not all(
- joint_name in locked_horizon for joint_name in JOINT_NAMES
- ):
- break
- previous_joint_target_positions_rad = np.array(
- [
- locked_horizon[joint_name][execution_index - 1]
- for joint_name in JOINT_NAMES
- ]
- )
- previous_joint_target_positions_deg = np.degrees(
- previous_joint_target_positions_rad
- )
- joint_errors = np.abs(
- current_joint_angles - previous_joint_target_positions_deg
- )
- if np.any(joint_errors <= MAX_ACTION_ERROR_THRESHOLD):
- break
- time.sleep(0.001)
-
- # Send current action to robot (if available)
- if all(joint_name in locked_horizon for joint_name in JOINT_NAMES):
- current_joint_target_positions_rad = np.array(
- [
- locked_horizon[joint_name][execution_index]
- for joint_name in JOINT_NAMES
- ]
- )
- current_joint_target_positions_deg = np.degrees(
- current_joint_target_positions_rad
- )
- # Update data_manager with target joint angles for visualization
- data_manager.set_target_joint_angles(
- current_joint_target_positions_deg
- )
-
- # Verify robot controller is enabled before sending commands
- if robot_controller.is_robot_enabled():
- robot_controller.set_target_joint_angles(
- current_joint_target_positions_deg
- )
- else:
- print(
- f"โ ๏ธ Robot controller not enabled, skipping command at index {execution_index}"
- )
-
- # Send current gripper open value to robot (if available)
- if GRIPPER_LOGGING_NAME in locked_horizon:
- current_gripper_target_open_value = locked_horizon[
- GRIPPER_LOGGING_NAME
- ][execution_index]
- robot_controller.set_gripper_open_value(
- current_gripper_target_open_value
- )
-
- # Update execution index
- policy_state.increment_execution_action_index()
-
- # Update status
- visualizer.update_policy_status(
- f"Executing policy: {execution_index + 1}/{locked_horizon_length}"
- )
- # 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,
- model_input_order,
- )
- 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)
-
- # 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"
- )
+# Extracted policy execution and lifecycle management actions
+from common.policy_actions import (
+ run_policy, start_policy_execution, play_policy, policy_execution_thread
+)
if __name__ == "__main__":
+ # ---------------------------------------------------------
+ # 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(
- "--ip-address",
- type=str,
- default=None,
- help="IP address of Meta Quest device (optional, defaults to None for auto-discovery)",
+ "--continuous-mode",
+ choices=["pipeline", "sequential"],
+ default="sequential",
+ help="Execution strategy for the receding horizon loop."
)
- policy_group = parser.add_mutually_exclusive_group(required=True)
- policy_group.add_argument(
- "--train-run-name",
- type=str,
- default=None,
- help="Name of the training run to load policy from (for cloud training).",
- )
- policy_group.add_argument(
- "--model-path",
- type=str,
- default=None,
- help="Path to local model file to load policy from.",
+ parser.add_argument(
+ "--robot-name",
+ type=str,
+ default="AgileX PiPER",
+ help="The registered hardware name in the Neuracore ecosystem."
)
- policy_group.add_argument(
- "--remote-endpoint-name",
- type=str,
- default=None,
- help="Name of remote Neuracore policy endpoint.",
+ 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="Cloud training run name.")
+ policy_group.add_argument("--model-path", type=str, default=None, help="Path to local .nc.zip model file.")
+ policy_group.add_argument("--remote-endpoint-name", type=str, default=None, help="Active remote inference endpoint.")
+
args = parser.parse_args()
- print("=" * 60)
- print("PIPER ROBOT TEST WITH NEURACORE POLICY")
- print("=" * 60)
- print("Thread frequencies:")
- print(f" ๐ฎ Quest Controller: {CONTROLLER_DATA_RATE} Hz")
- print(f" ๐งฎ IK Solver: {IK_SOLVER_RATE} Hz")
- print(f" ๐ค Robot Controller: {ROBOT_RATE} Hz")
- print(f" ๐ธ Camera Frame: {CAMERA_FRAME_STREAMING_RATE} Hz")
- print(f" ๐ Joint State: {JOINT_STATE_STREAMING_RATE} Hz")
- print(f" ๐ฅ๏ธ Visualization: {VISUALIZATION_RATE} Hz")
-
- # Connect to Neuracore
- print("\n๐ง Initializing Neuracore...")
- nc.login()
- nc.connect_robot(
- robot_name="AgileX PiPER",
- urdf_path=str(URDF_PATH),
- overwrite=False,
- )
+ print("=" * 60 + "\nPIPER ROBOT TEST WITH NEURACORE POLICY\n" + "=" * 60)
- # Load policy from either train run name or model path
- # NOTE: The model_output_order MUST match the exact order used during training
- # This order is determined by the output_robot_data_spec in the training config.
- # The order here should match the order in your training config's output_robot_data_spec.
- model_input_order = {
- DataType.JOINT_POSITIONS: JOINT_NAMES,
- DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_LOGGING_NAME],
- DataType.RGB_IMAGES: [CAMERA_LOGGING_NAME],
- }
- model_output_order = {
- DataType.JOINT_TARGET_POSITIONS: JOINT_NAMES,
- DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [GRIPPER_LOGGING_NAME],
- }
+ # ---------------------------------------------------------
+ # 2. Configuration & Neuracore Initialization
+ # ---------------------------------------------------------
+ # Load the YAML configuration dictionary
+ config = load_ik_config(args.ik_config)
- print("\n๐ Model input order:")
- for data_type, names in model_input_order.items():
- print(f" {data_type.name}: {names}")
- print("\n๐ Model output order:")
- for data_type, names in model_output_order.items():
- print(f" {data_type.name}: {names}")
+ nc.login()
+ nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False)
+ # ---------------------------------------------------------
+ # 3. Policy Loading & Embodiment Resolution
+ # ---------------------------------------------------------
if args.remote_endpoint_name is not None:
- print(
- f"\n๐ค Connecting to remote policy endpoint: {args.remote_endpoint_name}..."
- )
+ print(f"\n๐ค Connecting to remote policy endpoint: {args.remote_endpoint_name}...")
try:
policy = nc.policy_remote_server(args.remote_endpoint_name)
except nc.EndpointError:
@@ -735,217 +110,141 @@ 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",
- model_input_order=model_input_order,
- model_output_order=model_output_order,
+ 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",
- model_input_order=model_input_order,
- model_output_order=model_output_order,
+ robot_name=args.robot_name,
)
- print(" โ Policy loaded successfully")
- # 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
+ # ---------------------------------------------------------
+ # 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
)
- ik_thread.start()
- # Start camera thread
- print("\n๐ท Starting camera thread...")
- camera_thread_obj = threading.Thread(
- target=camera_thread, args=(data_manager,), daemon=True
- )
- camera_thread_obj.start()
-
- # Set up visualization
- print("\n๐ฅ๏ธ Starting Viser visualization...")
+ # ---------------------------------------------------------
+ # 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, model_input_order
- )
+ 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, model_input_order
- )
- )
visualizer.set_play_policy_callback(
- lambda: play_policy(
- data_manager, policy, policy_state, visualizer, model_input_order
- )
+ lambda: play_policy(data_manager, policy, policy_state, visualizer, 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())
- )
+ 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,
- data_manager,
- policy_state,
- robot_controller,
- visualizer,
- model_input_order,
- ),
+ args=(policy, data_manager, policy_state, robot_controller, visualizer, input_emb),
daemon=True,
+ name="PolicyExecutionWorker"
)
- policy_execution_thread_obj.start()
+ policy_exec_thread.start()
+ active_threads.append(policy_exec_thread)
- 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")
+ print("\n๐ System Online! Open http://localhost:8080 in your browser to visualize and run the policy.\n")
+ # ---------------------------------------------------------
+ # 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...")
-
- # Disconnect policy
+ # ---------------------------------------------------------
+ # 8. Graceful Teardown & Cleanup
+ # ---------------------------------------------------------
+ print("\n๐งน Cleaning up subsystems...")
+
+ # 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()
+
+ # 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.")
\ No newline at end of file
diff --git a/examples/5_rollout_neuracore_policy_minimal.py b/examples/5_rollout_neuracore_policy_minimal.py
index 8695337..67cb266 100644
--- a/examples/5_rollout_neuracore_policy_minimal.py
+++ b/examples/5_rollout_neuracore_policy_minimal.py
@@ -1,144 +1,64 @@
#!/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
import neuracore as nc
-import numpy as np
-from neuracore_types import (
- BatchedJointData,
- BatchedParallelGripperOpenAmountData,
- DataType,
-)
-# Add parent directory to path
+# ---------------------------------------------------------------------------
+# Path Configuration & Local Imports
+# ---------------------------------------------------------------------------
sys.path.insert(0, str(Path(__file__).parent.parent))
from common.configs import (
- CAMERA_LOGGING_NAME,
- GRIPPER_LOGGING_NAME,
- JOINT_NAMES,
- NEUTRAL_JOINT_ANGLES,
- POLICY_EXECUTION_RATE,
- PREDICTION_HORIZON_EXECUTION_RATIO,
- ROBOT_RATE,
- URDF_PATH,
+ CAMERA_NAMES, GRIPPER_NAME, URDF_PATH,
+ POLICY_EXECUTION_RATE, PREDICTION_HORIZON_EXECUTION_RATIO
)
-from common.data_manager import DataManager, RobotActivityState
-from common.policy_state import PolicyState
-from common.threads.camera import camera_thread
-from common.threads.joint_state import joint_state_thread
-
-from piper_controller import PiperController
-
-
-def convert_predictions_to_horizon_dict(predictions: dict) -> dict[str, list[float]]:
- """Convert predictions dict to horizon dict format."""
- horizon: dict[str, list[float]] = {}
-
- # Extract joint target positions
- if DataType.JOINT_TARGET_POSITIONS in predictions:
- joint_data = predictions[DataType.JOINT_TARGET_POSITIONS]
- for joint_name in JOINT_NAMES:
- if joint_name in joint_data:
- batched = joint_data[joint_name]
- if isinstance(batched, BatchedJointData):
- # Extract values: (B, T, 1) -> list[float], taking B=0
- values = batched.value[0, :, 0].cpu().numpy().tolist()
- horizon[joint_name] = values
-
- # Extract gripper open amounts
- if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in predictions:
- gripper_data = predictions[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS]
- if GRIPPER_LOGGING_NAME in gripper_data:
- batched = gripper_data[GRIPPER_LOGGING_NAME]
- if isinstance(batched, BatchedParallelGripperOpenAmountData):
- # Extract values: (B, T, 1) -> list[float], taking B=0
- values = batched.open_amount[0, :, 0].cpu().numpy().tolist()
- horizon[GRIPPER_LOGGING_NAME] = values
-
- return horizon
-
-
-def log_current_state(data_manager: DataManager) -> None:
- """Log current state to Neuracore."""
- current_joint_angles = data_manager.get_current_joint_angles()
- if current_joint_angles is None:
- print("โ ๏ธ No joint angles available")
- return
-
- # Get current gripper open value
- gripper_open_value = data_manager.get_current_gripper_open_value()
- if gripper_open_value is None:
- print("โ ๏ธ No gripper open value available")
- return
-
- # Get current RGB image
- rgb_image = data_manager.get_rgb_image()
- if rgb_image is None:
- print("โ ๏ธ No RGB image available")
- return
-
- # Prepare data for Neuracore logging
- joint_angles_rad = np.radians(current_joint_angles)
- joint_positions_dict = {
- joint_name: angle for joint_name, angle in zip(JOINT_NAMES, joint_angles_rad)
- }
-
- # Log joint positions, parallel gripper open amounts, and RGB image to Neuracore
- nc.log_joint_positions(joint_positions_dict)
- nc.log_parallel_gripper_open_amount(GRIPPER_LOGGING_NAME, gripper_open_value)
- nc.log_rgb(CAMERA_LOGGING_NAME, rgb_image)
-
-
-def run_policy(
- data_manager: DataManager,
- policy: nc.policy,
- policy_state: PolicyState,
-) -> bool:
- """Run policy and get prediction horizon."""
- # Log current state
- log_current_state(data_manager)
+from neuracore_types import DataType
- # Get policy prediction
- try:
- start_time = time.time()
- predictions = policy.predict(timeout=5)
- prediction_horizon = convert_predictions_to_horizon_dict(predictions)
- elapsed = time.time() - start_time
-
- # Get horizon length from the first joint (all should have same length)
- horizon_length = policy_state.get_prediction_horizon_length()
- print(f"โ Got {horizon_length} actions in {elapsed:.3f}s")
-
- policy_state.set_prediction_horizon(prediction_horizon)
- return True
-
- except Exception as e:
- print(f"โ Policy prediction failed: {e}")
- traceback.print_exc()
- return False
-
-
-def execute_horizon(
- data_manager: DataManager,
- policy_state: PolicyState,
- robot_controller: PiperController,
- frequency: int,
-) -> None:
- """Execute prediction horizon."""
+from common.data_manager import RobotActivityState
+from common.policy_helpers import (
+ get_policy_embodiments, print_policy_embodiments,
+ embodiment_names_ordered, gripper_open_at_index,
+ joint_targets_deg_at_index, log_robot_state_for_policy
+)
+from common.policy_state import PolicyState
+from common.config_parser import load_ik_config
+from common.system_bootstrap import bootstrap_robot_system
+from common.policy_actions import run_policy
+
+# ---------------------------------------------------------------------------
+# Helper Functions
+# ---------------------------------------------------------------------------
+def execute_horizon(data_manager, policy_state, robot_controller, frequency, input_embodiment, output_grippers):
+ """
+ Minimal terminal-only execution loop for the prediction horizon.
+
+ Locks the predicted horizon and streams the joint/gripper targets to the
+ robot at the specified frequency until the horizon is exhausted.
+ """
policy_state.start_policy_execution()
data_manager.set_robot_activity_state(RobotActivityState.POLICY_CONTROLLED)
@@ -149,239 +69,165 @@ def execute_horizon(
for i in range(horizon_length):
start_time = time.time()
- # Send current action to robot (if available)
- if all(joint_name in locked_horizon for joint_name in JOINT_NAMES):
- current_joint_target_positions_rad = np.array(
- [locked_horizon[joint_name][i] for joint_name in JOINT_NAMES]
- )
- current_joint_target_positions_deg = np.degrees(
- current_joint_target_positions_rad
- )
- robot_controller.set_target_joint_angles(current_joint_target_positions_deg)
-
- # Send current gripper open value to robot (if available)
- if GRIPPER_LOGGING_NAME in locked_horizon:
- current_gripper_target_open_value = locked_horizon[GRIPPER_LOGGING_NAME][i]
- robot_controller.set_gripper_open_value(current_gripper_target_open_value)
+ # 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)
- # Log current state for visualization
- log_current_state(data_manager)
+ # Dispatch gripper targets
+ gripper_target = gripper_open_at_index(locked_horizon, i, gripper_names=output_grippers)
+ if gripper_target is not None:
+ robot_controller.set_gripper_open_value(gripper_target)
- # Sleep to maintain rate
- elapsed = time.time() - start_time
- time.sleep(max(0, dt - elapsed))
+ # Log state back to the policy for the next inference cycle
+ log_robot_state_for_policy(data_manager, input_embodiment)
+
+ # Maintain loop frequency
+ time.sleep(max(0.0, dt - (time.time() - start_time)))
- # 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).",
- )
- policy_group.add_argument(
- "--model-path",
- type=str,
- default=None,
- help="Path to local model file to load policy from.",
- )
- policy_group.add_argument(
- "--remote-endpoint-name",
- type=str,
- default=None,
- help="Name of remote Neuracore policy endpoint.",
- )
- parser.add_argument(
- "--frequency",
- type=int,
- default=POLICY_EXECUTION_RATE,
- help="Frequency of policy execution",
- )
- parser.add_argument(
- "--execution-ratio",
- type=float,
- default=PREDICTION_HORIZON_EXECUTION_RATIO,
- help="Execution ratio of the policy",
- )
+ policy_group.add_argument("--train-run-name", type=str, default=None, help="Cloud training run name.")
+ policy_group.add_argument("--model-path", type=str, default=None, help="Path to local .nc.zip model file.")
+ policy_group.add_argument("--remote-endpoint-name", type=str, default=None, help="Active remote inference endpoint.")
+
+ parser.add_argument("--robot-name", type=str, default="AgileX PiPER")
+ parser.add_argument("--frequency", type=int, default=POLICY_EXECUTION_RATE)
+ parser.add_argument("--execution-ratio", type=float, default=PREDICTION_HORIZON_EXECUTION_RATIO)
+ parser.add_argument("--ik-config", type=str, default="ik_conf/default.yaml", help="Path to YAML configuration.")
args = parser.parse_args()
- print("=" * 60)
- 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="AgileX PiPER",
- urdf_path=str(URDF_PATH),
- overwrite=False,
- )
-
- # Load policy
- # NOTE: The model_output_order MUST match the exact order used during training
- # This order is determined by the output_robot_data_spec in the training config.
- # The order here should match the order in your training config's output_robot_data_spec.
- model_input_order = {
- DataType.JOINT_POSITIONS: JOINT_NAMES,
- DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_LOGGING_NAME],
- DataType.RGB_IMAGES: [CAMERA_LOGGING_NAME],
- }
- model_output_order = {
- DataType.JOINT_TARGET_POSITIONS: JOINT_NAMES,
- DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [GRIPPER_LOGGING_NAME],
- }
-
- print("\n๐ Model input order:")
- for data_type, names in model_input_order.items():
- print(f" {data_type.name}: {names}")
- print("\n๐ Model output order:")
- for data_type, names in model_output_order.items():
- print(f" {data_type.name}: {names}")
+ nc.connect_robot(robot_name=args.robot_name, urdf_path=str(URDF_PATH), overwrite=False)
- if args.remote_endpoint_name is not None:
- print(
- f"\n๐ค Connecting to remote policy endpoint: {args.remote_endpoint_name}..."
- )
+ # 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:
- print(
- f"โ Endpoint '{args.remote_endpoint_name}' not available. "
- "Please start it from the Neuracore dashboard."
- )
+ except Exception as e:
+ # Catch the EndpointError and provide explicit, actionable instructions
+ print("\n" + "!" * 60)
+ print(f"โ ENDPOINT NOT ACTIVE: '{args.remote_endpoint_name}'")
+ print("!" * 60)
+ print("The script successfully reached Neuracore, but the remote server is down.")
+ print("\nPLEASE FOLLOW THESE STEPS:")
+ print(" 1. Open your browser and go to the Neuracore website/dashboard.")
+ print(f" 2. Locate your deployment endpoint named '{args.remote_endpoint_name}'.")
+ print(" 3. Click 'Deploy' or 'Activate' to spin up the cloud server.")
+ print(" 4. Wait for the status to show as 'Active', then rerun this script.")
+ print("!" * 60 + "\n")
sys.exit(1)
- elif args.train_run_name is not None:
- print(f"\n๐ค Loading policy from training run: {args.train_run_name}...")
- policy = nc.policy(
- train_run_name=args.train_run_name,
- model_input_order=model_input_order,
- model_output_order=model_output_order,
- )
+
+ 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}...")
- policy = nc.policy(
- model_file=args.model_path,
- device="cuda",
- model_input_order=model_input_order,
- model_output_order=model_output_order,
- )
- print(" โ Policy loaded successfully")
-
- # Initialize state
- data_manager = DataManager()
+ 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)
+
+ # 3. Embodiment Extraction with Remote Fallback
+ try:
+ input_emb, output_emb = get_policy_embodiments(policy)
+ except AttributeError:
+ print("\nโ ๏ธ Could not dynamically extract embodiments. Using default Piper configuration...")
+ input_emb = {
+ DataType.JOINT_POSITIONS: {i: f"joint{i+1}" for i in range(6)},
+ DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: {0: GRIPPER_NAME},
+ DataType.RGB_IMAGES: {0: CAMERA_NAMES[0]},
+ }
+ output_emb = {
+ DataType.JOINT_TARGET_POSITIONS: {i: f"joint{i+1}" for i in range(6)},
+ DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: {0: GRIPPER_NAME},
+ }
+
+ print_policy_embodiments(input_emb, output_emb)
+
+ output_gripper_names = None
+ if output_emb and DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in output_emb:
+ output_gripper_names = embodiment_names_ordered(output_emb[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS])
+
+ # 4. Bootstrap Core System (No Quest, No IK needed for minimal playback)
+ data_manager, robot_controller, _, active_threads = bootstrap_robot_system(
+ config, start_ik=False, start_camera=True
+ )
+
policy_state = PolicyState()
policy_state.set_execution_ratio(args.execution_ratio)
- # Initialize robot controller
- print("\n๐ค Initializing robot controller...")
- robot_controller = PiperController(
- can_interface="can0",
- robot_rate=ROBOT_RATE,
- control_mode=PiperController.ControlMode.JOINT_SPACE,
- neutral_joint_angles=NEUTRAL_JOINT_ANGLES,
- debug_mode=False,
- )
- robot_controller.start_control_loop()
-
- # Start joint state thread
- print("\n๐ Starting joint state thread...")
- joint_state_thread_obj = threading.Thread(
- target=joint_state_thread, args=(data_manager, robot_controller), daemon=True
- )
- joint_state_thread_obj.start()
-
- # Start camera thread
- print("\n๐ท Starting camera thread...")
- camera_thread_obj = threading.Thread(
- target=camera_thread, args=(data_manager,), daemon=True
- )
- camera_thread_obj.start()
-
- # Wait for threads to initialize
- print("\nโณ Waiting for initialization...")
- time.sleep(2.0)
+ time.sleep(2.0) # Wait for 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)
-
- # Wait for homing to complete
- start_time = time.time()
- while (
- data_manager.get_robot_activity_state() == RobotActivityState.HOMING
- and not robot_controller.is_robot_homed()
- and time.time() - start_time < 5.0
- ):
+ robot_controller.move_to_home()
+
+ while data_manager.get_robot_activity_state() == RobotActivityState.HOMING and not robot_controller.is_robot_homed():
time.sleep(0.1)
- print("โ Robot homed")
-
- # Policy execution loop
- print("\n๐ Starting policy execution loop...")
- print("Press Ctrl+C to stop\n")
+
+ data_manager.set_robot_activity_state(RobotActivityState.ENABLED)
+ print("โ Robot ready. Starting policy execution loop... Press Ctrl+C to stop.\n")
while True:
- # Run policy
- if not run_policy(data_manager, policy, policy_state):
- print("โ ๏ธ Policy run failed, retrying...")
+ # Query the network for the next prediction
+ if not run_policy(data_manager, policy, policy_state, visualizer=None, input_embodiment_description=input_emb):
+ print("โ ๏ธ Policy 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
+ data_manager, policy_state, robot_controller,
+ args.frequency, 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")
-
- # Shutdown
+ print("\n๐งน Cleaning up subsystems...")
+ try:
+ data_manager.set_robot_activity_state(RobotActivityState.HOMING)
+ robot_controller.move_to_home()
+ # Wait safely for homing to finish before severing the hardware connection
+ for _ in range(10):
+ if robot_controller.is_robot_homed():
+ break
+ time.sleep(1)
+ except Exception:
+ pass
+
policy.disconnect()
data_manager.set_robot_activity_state(RobotActivityState.DISABLED)
data_manager.request_shutdown()
- joint_state_thread_obj.join()
- camera_thread_obj.join()
- time.sleep(0.5) # Give threads time to stop
-
- robot_controller.cleanup()
+
+ for t in active_threads:
+ t.join(timeout=2.0)
+
+ if robot_controller:
+ try:
+ robot_controller.graceful_stop()
+ except AttributeError:
+ pass
+ robot_controller.cleanup()
+
nc.logout()
-
- print("โ Cleanup complete")
- print("\n๐ Done.")
+ print("๐ Shutdown complete.")
\ No newline at end of file
diff --git a/examples/6_visualize_policy_from_dataset.py b/examples/6_visualize_policy_from_dataset.py
index 633f6d9..4ba32cf 100644
--- a/examples/6_visualize_policy_from_dataset.py
+++ b/examples/6_visualize_policy_from_dataset.py
@@ -1,311 +1,240 @@
#!/usr/bin/env python3
-"""Simple policy visualization from dataset.
+"""
+Offline Policy Visualization and Validation.
+
+This script acts as a safe, simulated testing ground for your trained AI policies.
+Instead of running on the physical robot, it:
+ 1. Loads a synchronized teleoperation dataset from Neuracore.
+ 2. Selects a random step (observation) from the dataset.
+ 3. Feeds that real-world camera and joint data into the trained policy.
+ 4. Renders the AI's predicted future action horizon in a 3D Viser web UI.
-Loads a policy and a dataset, and randomly selects a state
-from the dataset to run the policy with and visualize the results.
+It is highly recommended to run this script to visually validate a model's sanity
+before deploying it to the physical AgileX Piper arm (via Scripts 4 or 5).
+
+Usage Examples:
+ python 6_visualize_policy_from_dataset.py --dataset-name my_dataset --model-path ./model.nc.zip
+ python 6_visualize_policy_from_dataset.py --dataset-name my_dataset --train-run-name cloud_run
"""
import argparse
import random
import sys
import time
+import traceback
+import logging
from pathlib import Path
+# ---------------------------------------------------------------------------
+# Suppress Noisy WebRTC/STUN Networking Errors from Viser
+# ---------------------------------------------------------------------------
+# Viser attempts to use STUN servers for WebRTC streaming. If a firewall/VPN
+# blocks it, it throws massive 401 errors before falling back to WebSockets.
+# This silences those unhandled asyncio exceptions so the terminal stays clean.
+logging.getLogger("asyncio").setLevel(logging.CRITICAL)
+logging.getLogger("aioice").setLevel(logging.CRITICAL)
+logging.getLogger("aiortc").setLevel(logging.CRITICAL)
+
import neuracore as nc
import numpy as np
import viser
import yourdfpy
-from neuracore_types import (
- BatchedJointData,
- BatchedNCData,
- BatchedParallelGripperOpenAmountData,
- DataType,
- RobotDataSpec,
-)
from PIL import Image
from viser.extras import ViserUrdf
-# Add parent directory to path
+# ---------------------------------------------------------------------------
+# Path Configuration & Local Imports
+# ---------------------------------------------------------------------------
sys.path.insert(0, str(Path(__file__).parent.parent))
from common.configs import (
- CAMERA_LOGGING_NAME,
- GRIPPER_LOGGING_NAME,
- JOINT_NAMES,
- URDF_PATH,
-)
-
-# Parse arguments
-parser = argparse.ArgumentParser(
- description="Visualize policy predictions from dataset"
+ JOINT_NAMES, POLICY_EXECUTION_RATE, URDF_PATH, CAMERA_NAMES, GRIPPER_NAME
)
-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"
+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,
)
-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(
- "--frequency", type=int, default=100, help="Frequency of visualization"
-)
-args = parser.parse_args()
-
-# Connect to Neuracore
-print("๐ง Initializing Neuracore...")
-nc.login()
-nc.connect_robot(robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False)
-
-# Load policy
-model_input_order = {
- DataType.JOINT_POSITIONS: JOINT_NAMES,
- DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: [GRIPPER_LOGGING_NAME],
- DataType.RGB_IMAGES: [CAMERA_LOGGING_NAME],
-}
-model_output_order = {
- DataType.JOINT_TARGET_POSITIONS: JOINT_NAMES,
- DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: [GRIPPER_LOGGING_NAME],
-}
-
-if args.remote_endpoint_name:
- print(f"๐ค Connecting to remote policy endpoint: {args.remote_endpoint_name}...")
+from neuracore_types import DataType
+
+
+if __name__ == "__main__":
+ # ---------------------------------------------------------
+ # 1. Argument Parsing
+ # ---------------------------------------------------------
+ parser = argparse.ArgumentParser(description="Visualize policy predictions offline using dataset states.")
+ parser.add_argument("--dataset-name", type=str, required=True, help="Neuracore dataset to draw observations from.")
+
+ 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 as e:
+ print("\n" + "!" * 60)
+ print(f"โ ENDPOINT NOT ACTIVE: '{args.remote_endpoint_name}'")
+ print("!" * 60)
+ print("The script successfully reached Neuracore, but the remote server is down.")
+ print("\nPLEASE FOLLOW THESE STEPS:")
+ print(" 1. Open your browser and go to the Neuracore website/dashboard.")
+ print(f" 2. Locate your deployment endpoint named '{args.remote_endpoint_name}'.")
+ print(" 3. Click 'Deploy' or 'Activate' to spin up the cloud server.")
+ print(" 4. Wait for the status to show as 'Active', then rerun this script.")
+ print("!" * 60 + "\n")
+ sys.exit(1)
+
+ elif args.train_run_name:
+ print(f"\n๐ค Loading policy from cloud training run: {args.train_run_name}...")
+ policy = nc.policy(train_run_name=args.train_run_name, device="cuda", robot_name=args.robot_name)
+ else:
+ print(f"\n๐ค Loading policy from local model: {args.model_path}...")
+ policy = nc.policy(model_file=args.model_path, device="cuda", robot_name=args.robot_name)
+
+ # ---------------------------------------------------------
+ # 3. Embodiment Extraction & Fallback
+ # ---------------------------------------------------------
try:
- policy = nc.policy_remote_server(args.remote_endpoint_name)
- except nc.EndpointError:
- print(
- f"โ Endpoint '{args.remote_endpoint_name}' not available. "
- "Please start it from the Neuracore dashboard."
- )
- sys.exit(1)
-elif args.train_run_name:
- print(f"๐ค Loading policy from training run: {args.train_run_name}...")
- policy = nc.policy(
- train_run_name=args.train_run_name,
- device="cuda",
- model_input_order=model_input_order,
- model_output_order=model_output_order,
- )
-else:
- print(f"๐ค Loading policy from model file: {args.model_path}...")
- policy = nc.policy(
- model_file=args.model_path,
- device="cuda",
- model_input_order=model_input_order,
- model_output_order=model_output_order,
+ input_emb, output_emb = get_policy_embodiments(policy)
+ except AttributeError:
+ print("\nโ ๏ธ Could not dynamically extract embodiments. Using default Piper configuration...")
+ input_emb = {
+ DataType.JOINT_POSITIONS: {i: f"joint{i+1}" for i in range(6)},
+ DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS: {0: GRIPPER_NAME},
+ DataType.RGB_IMAGES: {0: CAMERA_NAMES[0]},
+ }
+ output_emb = {
+ DataType.JOINT_TARGET_POSITIONS: {i: f"joint{i+1}" for i in range(6)},
+ DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS: {0: GRIPPER_NAME},
+ }
+
+ print_policy_embodiments(input_emb, output_emb)
+
+ out_grippers = (
+ embodiment_names_ordered(output_emb[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS])
+ if output_emb and DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in output_emb
+ else None
)
-print(" โ Policy loaded")
-
-# 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")
-
-# Get data types from model input and output
-required_data_types = set(model_input_order.keys()) | set(model_output_order.keys())
-
-# Filter data spec to only include required data types
-robot_data_spec: RobotDataSpec = {
- robot_id: {
- data_type: names
- for data_type, names in dataset.get_full_data_spec(robot_id).items()
- if data_type in required_data_types
- }
- for robot_id in dataset.robot_ids
-}
-
-print("๐ Synchronizing dataset...")
-synced_dataset = dataset.synchronize(
- frequency=args.frequency,
- robot_data_spec=robot_data_spec,
- 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
-
-
-def convert_predictions_to_horizon(
- predictions: dict[DataType, dict[str, BatchedNCData]],
-) -> dict[str, list[float]]:
- """Convert predictions to horizon dict."""
- horizon = {}
- if DataType.JOINT_TARGET_POSITIONS in predictions:
- joint_data = predictions[DataType.JOINT_TARGET_POSITIONS]
- for joint_name in JOINT_NAMES:
- if joint_name in joint_data:
- batched = joint_data[joint_name]
- if isinstance(batched, BatchedJointData):
- values = batched.value[0, :, 0].cpu().numpy().tolist()
- horizon[joint_name] = values
- if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in predictions:
- gripper_data = predictions[DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS]
- if GRIPPER_LOGGING_NAME in gripper_data:
- batched = gripper_data[GRIPPER_LOGGING_NAME]
- if isinstance(batched, BatchedParallelGripperOpenAmountData):
- values = batched.open_amount[0, :, 0].cpu().numpy().tolist()
- horizon[GRIPPER_LOGGING_NAME] = values
- return horizon
-
-def select_random_state() -> None:
- """Select random state and run policy."""
- global current_horizon, current_action_idx, playing
-
- # Select random episode and step
- 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}")
-
- # Extract joint positions
- joint_positions_dict = {}
- if DataType.JOINT_POSITIONS in step.data:
- joint_data = step.data[DataType.JOINT_POSITIONS]
- for joint_name in JOINT_NAMES:
- if joint_name in joint_data:
- joint_positions_dict[joint_name] = joint_data[joint_name].value
- # Log to Neuracore for visualization
- nc.log_joint_positions(joint_positions_dict)
-
- # Extract gripper
- gripper_value = 1.0
- if DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS in step.data:
- gripper_data = step.data[DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS]
- if GRIPPER_LOGGING_NAME in gripper_data:
- gripper_value = gripper_data[GRIPPER_LOGGING_NAME].open_amount
- # Log to Neuracore for visualization
- nc.log_parallel_gripper_open_amount(GRIPPER_LOGGING_NAME, gripper_value)
-
- # Extract RGB image
- rgb_image = None
- if DataType.RGB_IMAGES in step.data:
- rgb_data = step.data[DataType.RGB_IMAGES]
- if CAMERA_LOGGING_NAME in rgb_data:
- rgb_image = np.array(rgb_data[CAMERA_LOGGING_NAME].frame)
- # Save image to file for visualization
- image_pil = Image.fromarray(rgb_image)
- image_pil.save("current_image.png")
- print("๐พ Saved image to current_image.png")
- # Log to Neuracore for visualization
- nc.log_rgb(CAMERA_LOGGING_NAME, rgb_image)
-
- # Get policy prediction
- print("๐ฏ Getting policy prediction...")
- predictions = policy.predict(timeout=5)
- current_horizon = convert_predictions_to_horizon(predictions)
- current_action_idx = 0
- playing = True
- print("FINISHED PREDICTION")
-
- # Update robot to initial pose from first step in the horizon
-
- joint_positions = np.array([current_horizon[jn][0] for jn in JOINT_NAMES])
- urdf_vis.update_cfg(joint_positions)
-
- print(
- f"โ
Prediction received: {len(current_horizon.get(JOINT_NAMES[0], []))} actions"
+ # ---------------------------------------------------------
+ # 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
)
+
+ if len(synced_dataset) == 0:
+ print("โ Error: The synchronized dataset is empty. Exiting.")
+ sys.exit(1)
-
-# 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, # Read-only
-)
-
-# 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 initial state
-select_random_state()
-# Main loop
-try:
- while True:
- start_time = time.time()
-
- # Update robot visualization
- if (
- playing
- and current_horizon
- and len(current_horizon.get(JOINT_NAMES[0], [])) > 0
- ):
- horizon_length = len(current_horizon[JOINT_NAMES[0]])
- if current_action_idx < horizon_length:
- # Get current action
- joint_config = np.array(
- [
- current_horizon[joint_name][current_action_idx]
- for joint_name in JOINT_NAMES
- ]
- )
- urdf_vis.update_cfg(joint_config)
-
- # Log to Neuracore for visualization
- # NOTE: we log to joint positions instead of joint target positions
- # because the latter is not visualized by Neuracore
- joint_config_dict = {
- jn: joint_config[i] for i, jn in enumerate(JOINT_NAMES)
- }
- nc.log_joint_positions(joint_config_dict)
-
- # Update gripper value
- gripper_value = current_horizon[GRIPPER_LOGGING_NAME][
- current_action_idx
- ]
- gripper_handle.value = round(
- gripper_value, 2
- ) # Round to 2 decimal places
-
- # Advance to next action
- current_action_idx = (current_action_idx + 1) % horizon_length
-
- # Sleep to control update rate
- elapsed = time.time() - start_time
- frequency = max(frequency_handle.value, 0.1) # Avoid division by zero
- time.sleep(max(0, 1.0 / frequency - elapsed))
-
-except KeyboardInterrupt:
- print("\n๐ Shutting down...")
-finally:
- policy.disconnect()
- nc.logout()
+ # ---------------------------------------------------------
+ # 5. Viser 3D UI Setup
+ # ---------------------------------------------------------
+ print("\n๐ฅ๏ธ Starting Viser simulation environment...")
+ server = viser.ViserServer()
+ server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1)
+
+ urdf_vis = ViserUrdf(server, yourdfpy.URDF.load(str(URDF_PATH)), root_node_name="/robot")
+ urdf_vis.update_cfg(np.zeros(len(JOINT_NAMES)))
+
+ state = {"horizon": None, "action_idx": 0, "playing": False, "rgb_handle": None}
+
+ def select_random_state() -> None:
+ """Pulls a random observation from the dataset and queries the AI policy."""
+ episode = synced_dataset[random.randint(0, len(synced_dataset) - 1)]
+ if not len(episode):
+ return
+ 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:
+ state["rgb_handle"].image = rgb_arr
+ break
+
+ 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:
+ while True:
+ start_time = time.time()
+ h_len = horizon_length(state["horizon"] or {})
+
+ if state["playing"] and state["horizon"] and h_len > 0:
+ if state["action_idx"] < h_len:
+ j_cfg = urdf_cfg_from_horizon(state["horizon"], state["action_idx"])
+ if j_cfg is not None:
+ urdf_vis.update_cfg(j_cfg)
+ nc.log_joint_positions({jn: float(j_cfg[i]) for i, jn in enumerate(JOINT_NAMES)})
+
+ g_val = gripper_open_at_index(state["horizon"], state["action_idx"], out_grippers)
+ if g_val is not None:
+ gripper_handle.value = round(g_val, 2)
+
+ state["action_idx"] = (state["action_idx"] + 1) % h_len
+
+ time.sleep(max(0.0, 1.0 / max(freq_handle.value, 0.1) - (time.time() - start_time)))
+
+ except KeyboardInterrupt:
+ print("\n๐ Interrupt received. Shutting down gracefully...")
+ except Exception as e:
+ print(f"\nโ Unhandled error in rendering loop: {e}")
+ traceback.print_exc()
+
+ # ---------------------------------------------------------
+ # 7. Cleanup
+ # ---------------------------------------------------------
+ finally:
+ print("\n๐งน Severing backend connections...")
+ policy.disconnect()
+ nc.logout()
+ print("๐ Offline validation complete.")
\ No newline at end of file
diff --git a/examples/7_teleop_with_pedal.py b/examples/7_teleop_with_pedal.py
new file mode 100644
index 0000000..a377301
--- /dev/null
+++ b/examples/7_teleop_with_pedal.py
@@ -0,0 +1,195 @@
+#!/usr/bin/env python3
+"""
+Piper Robot Teleoperation with Meta Quest and Foot Pedals.
+
+This script provides a unified control interface for the AgileX Piper robot,
+combining the 6D spatial tracking of a Meta Quest controller with the
+hands-free convenience of a USB foot pedal. It simultaneously streams
+synchronized telemetry data to the Neuracore cloud.
+
+Key Features:
+ - Meta Quest for precise 6-DoF end-effector manipulation.
+ - Foot Pedals mapped to system states (Enable/Disable, Home, Record).
+ - Dynamically loads IK and scaling configurations from a YAML file.
+ - Robust ADB connection handling for the VR headset.
+
+Hardware Requirements:
+ - AgileX Piper robot arm connected via CAN interface ('can0').
+ - Meta Quest headset connected via USB with 'USB Debugging' explicitly allowed.
+ - 3-Button USB Foot Pedal (mapped to keystrokes 'a', 'b', 'c').
+
+Usage:
+ python 7_teleop_with_pedal.py --ip-address --ik-config ik_conf/default.yaml
+"""
+
+import argparse
+import multiprocessing
+import sys
+import threading
+import time
+import traceback
+from pathlib import Path
+
+import neuracore as nc
+
+# ---------------------------------------------------------------------------
+# Path Configuration & Local Imports
+# ---------------------------------------------------------------------------
+sys.path.insert(0, str(Path(__file__).parent.parent))
+
+from common.configs import URDF_PATH, META_QUEST_AXIS_MASK
+from common.config_parser import load_ik_config
+from common.data_manager import RobotActivityState
+from common.system_bootstrap import bootstrap_robot_system
+from common.shared_actions import (
+ toggle_robot_enabled, move_robot_home,
+ toggle_recording, neuracore_logging_callback
+)
+from common.foot_pedal import FootPedal
+from common.threads.quest_reader import quest_reader_thread
+from meta_quest_teleop.reader import MetaQuestReader
+
+# Hardware key mappings for the USB foot pedal
+ENABLE_DISABLE_PEDAL = "a"
+HOME_POSITION_PEDAL = "b"
+RECORD_TOGGLE_PEDAL = "c"
+
+if __name__ == "__main__":
+ # Ensure safe multiprocess spawning for UI/Background threads
+ multiprocessing.set_start_method("spawn")
+
+ # ---------------------------------------------------------
+ # 1. Argument Parsing
+ # ---------------------------------------------------------
+ parser = argparse.ArgumentParser(description="Combined Quest + Pedal Teleop")
+ parser.add_argument("--ip-address", type=str, default=None, help="Meta Quest IP address on the local network.")
+ parser.add_argument("--dataset-name", type=str, default=None, help="Override the auto-generated dataset name.")
+ parser.add_argument("--ik-config", type=str, default="ik_conf/default.yaml", help="Path to IK/teleop YAML config.")
+ args = parser.parse_args()
+
+ print("=" * 60 + "\nPIPER TELEOP: META QUEST + FOOT PEDALS\n" + "=" * 60)
+
+ # Load the YAML configuration dictionary
+ config = load_ik_config(args.ik_config)
+
+ # ---------------------------------------------------------
+ # 2. Neuracore Initialization & Dataset Creation
+ # ---------------------------------------------------------
+ print("\n๐ง Initializing Neuracore...")
+ try:
+ nc.login()
+ nc.connect_robot(robot_name="AgileX PiPER", urdf_path=str(URDF_PATH), overwrite=False)
+
+ ds_name = args.dataset_name or f"pedal-teleop-{time.strftime('%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}")
+
+ # ---------------------------------------------------------
+ # 3. Hardware & Subsystem Bootstrapping
+ # ---------------------------------------------------------
+ data_manager, robot_controller, ik_solver, active_threads = bootstrap_robot_system(
+ config, start_ik=True, start_camera=True
+ )
+
+ # Wire Neuracore logging to DataManager so states push to the cloud
+ data_manager.set_on_change_callback(neuracore_logging_callback)
+
+ # ---------------------------------------------------------
+ # 4. Meta Quest Initialization & Error Handling
+ # ---------------------------------------------------------
+ print("\n๐ฎ Initializing Meta Quest reader...")
+ quest_reader = None
+ try:
+ quest_reader = MetaQuestReader(
+ ip_address=args.ip_address, port=5555, run=True, axis_mask=META_QUEST_AXIS_MASK
+ )
+ quest_thread = threading.Thread(target=quest_reader_thread, args=(data_manager, quest_reader), daemon=True)
+ quest_thread.start()
+ active_threads.append(quest_thread)
+ except (Exception, SystemExit) as e:
+ print("\n" + "!" * 60)
+ print("โ FAILED TO ACCESS META QUEST")
+ print("!" * 60)
+ print("The headset is plugged in, but ADB debugging permissions are missing.")
+ print("\nPLEASE FOLLOW THESE STEPS:")
+ print(" 1. Put the Meta Quest headset on your head.")
+ print(" 2. Look for a notification in your menu that says 'USB Detected'.")
+ print(" 3. Click on that notification and select 'Allow' to grant data access.")
+ print(" 4. Rerun this script.")
+ print("!" * 60 + "\n")
+
+ data_manager.request_shutdown()
+ for t in active_threads:
+ t.join(timeout=1.0)
+ if robot_controller:
+ robot_controller.cleanup()
+ sys.exit(1)
+
+ # ---------------------------------------------------------
+ # 5. Foot Pedal Initialization & Binding
+ # ---------------------------------------------------------
+ print("\nโจ๏ธ Initializing Foot Pedals...")
+ pedal = FootPedal({
+ "button_a": ENABLE_DISABLE_PEDAL,
+ "button_b": HOME_POSITION_PEDAL,
+ "button_c": RECORD_TOGGLE_PEDAL,
+ })
+
+ # 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("๐ฎ 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:
+ # Keep the main thread alive while background threads handle execution
+ while not data_manager.is_shutdown_requested():
+ time.sleep(1)
+
+ except KeyboardInterrupt:
+ print("\n๐ Interrupt received - shutting down gracefully...")
+ data_manager.request_shutdown()
+ except Exception as e:
+ print(f"\nโ Unhandled Demo error: {e}")
+ traceback.print_exc()
+ data_manager.request_shutdown()
+
+ # ---------------------------------------------------------
+ # 7. Graceful Teardown & Cleanup
+ # ---------------------------------------------------------
+ finally:
+ print("\n๐งน Cleaning up subsystems...")
+
+ # Ensure we don't leave dangling, unterminated recordings on the cloud
+ if nc.is_recording():
+ nc.cancel_recording()
+
+ try:
+ nc.logout()
+ except Exception:
+ pass
+
+ data_manager.set_robot_activity_state(RobotActivityState.DISABLED)
+ data_manager.request_shutdown()
+
+ if quest_reader:
+ quest_reader.stop()
+
+ for t in active_threads:
+ t.join(timeout=2.0)
+
+ if robot_controller:
+ robot_controller.cleanup()
+
+ print("๐ Demo stopped.")
\ No newline at end of file
diff --git a/examples/combine_code.py b/examples/combine_code.py
new file mode 100644
index 0000000..043cf46
--- /dev/null
+++ b/examples/combine_code.py
@@ -0,0 +1,54 @@
+import pathlib
+
+def combine_python_files(directory_path, output_filename):
+ """
+ Recursively finds all .py files in a directory and combines them into a single text file.
+
+ Args:
+ directory_path (str): The path to the root directory to search.
+ output_filename (str): The name/path of the output text file.
+ """
+ # Create a Path object for the target directory
+ source_dir = pathlib.Path(directory_path)
+
+ # Ensure the directory exists
+ if not source_dir.is_dir():
+ print(f"Error: The directory '{directory_path}' does not exist.")
+ return
+
+ # Open the output file in write mode
+ with open(output_filename, 'w', encoding='utf-8') as outfile:
+ # rglob('*.py') recursively searches for all files ending in .py
+ py_files = list(source_dir.rglob('*.py'))
+
+ if not py_files:
+ print(f"No Python files found in '{directory_path}'.")
+ return
+
+ print(f"Found {len(py_files)} Python files. Combining...")
+
+ for file_path in py_files:
+ # Write a clear separator and the file path as a header
+ outfile.write(f"\n{'='*60}\n")
+ outfile.write(f"FILE: {file_path}\n")
+ outfile.write(f"{'='*60}\n\n")
+
+ # Read the python file and append its contents
+ try:
+ with open(file_path, 'r', encoding='utf-8') as infile:
+ outfile.write(infile.read())
+ outfile.write("\n")
+ except Exception as e:
+ error_msg = f"Error reading file {file_path}: {e}\n"
+ print(error_msg)
+ outfile.write(error_msg)
+
+ print(f"Success! All files have been combined into '{output_filename}'.")
+
+# --- Example Usage ---
+if __name__ == "__main__":
+ # Replace these variables with your actual paths
+ TARGET_DIRECTORY = "./" # "./" means current directory
+ OUTPUT_FILE = "combined_code.txt"
+
+ combine_python_files(TARGET_DIRECTORY, OUTPUT_FILE)
\ No newline at end of file
diff --git a/examples/common/config_parser.py b/examples/common/config_parser.py
new file mode 100644
index 0000000..cab642f
--- /dev/null
+++ b/examples/common/config_parser.py
@@ -0,0 +1,12 @@
+import yaml
+from pathlib import Path
+
+def load_ik_config(config_path: str) -> dict:
+ """Loads the teleoperation and IK parameters from a YAML file."""
+ path = Path(config_path)
+ if not path.exists():
+ print(f"โ ๏ธ Warning: Config file not found at {path}. Using safe fallback defaults.")
+ return {}
+
+ with open(path, 'r') as f:
+ return yaml.safe_load(f)
\ No newline at end of file
diff --git a/examples/common/configs.py b/examples/common/configs.py
index d2ac818..fd5646d 100644
--- a/examples/common/configs.py
+++ b/examples/common/configs.py
@@ -1,63 +1,43 @@
-"""Configuration parameters for Piper robot demos."""
-
+"""Configuration parameters for AgileX Piper robot demos."""
from pathlib import Path
-URDF_PATH = str(
- Path(__file__).parent.parent.parent
- / "piper_description"
- / "urdf"
- / "piper_description.urdf"
-)
-
+# Hardware & Kinematics Paths
+URDF_PATH = str(Path(__file__).parent.parent.parent / "piper_description" / "urdf" / "piper_description.urdf")
GRIPPER_FRAME_NAME = "gripper_center"
-
-# Pink IK parameters
+# IK Solver
SOLVER_NAME = "quadprog"
-POSITION_COST = 1.0
-ORIENTATION_COST = 0.75
-FRAME_TASK_GAIN = 0.4
-LM_DAMPING = 0.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 = 3.0
-ROTATION_SCALE = 2.0
-
-# 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
-ROBOT_RATE = 100.0
-
-# Neuracore data collection rates
-JOINT_STATE_STREAMING_RATE = 100.0 # Data collection rate for neuracore
-CAMERA_FRAME_STREAMING_RATE = 60.0 # Data collection rate for camera frame
-
-# # Initial neutral pose for robot (degrees)
-NEUTRAL_JOINT_ANGLES = [-1.003, 80.167, -51.064, -4.127, 16.548, 2.619]
-
-# Posture task cost vector (one weight per joint)
-POSTURE_COST_VECTOR = [0.0, 0.0, 0.0, 0.05, 0.0, 0.0]
-
-
-POLICY_EXECUTION_RATE = 100.0 # Hz
-PREDICTION_HORIZON_EXECUTION_RATIO = (
- 0.8 # percentage of the prediction horizon that is executed
-)
-MAX_SAFETY_THRESHOLD = 20.0 # degrees
-MAX_ACTION_ERROR_THRESHOLD = 3.0 # degrees
-TARGETING_POSE_TIME_THRESHOLD = 1.0 # seconds
-
-GRIPPER_LOGGING_NAME = "gripper"
-CAMERA_LOGGING_NAME = "rgb"
+
+# Teleoperation Thresholds
+GRIP_THRESHOLD = 0.9
+
+# Thread Execution Rates (Hz)
+CONTROLLER_DATA_RATE = 50.0
+IK_SOLVER_RATE = 100
+VISUALIZATION_RATE = 60.0
+ROBOT_RATE = 100.0
+JOINT_STATE_STREAMING_RATE = 100.0
+CAMERA_FRAME_STREAMING_RATE = 30.0
+
+# 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"]
+
+# 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
+
+# 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
JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]
diff --git a/examples/common/data_manager.py b/examples/common/data_manager.py
index 9e159a1..82a7ec9 100644
--- a/examples/common/data_manager.py
+++ b/examples/common/data_manager.py
@@ -4,20 +4,31 @@
This module provides shared state classes for teleoperation systems that need
to coordinate between multiple threads (data collection, IK solving, visualization).
"""
-
import threading
import time
+import queue # Added for async queueing
from enum import Enum
from typing import Any, Callable
import numpy as np
+from .configs import GRIPPER_NAME, JOINT_NAMES
from .one_euro_filter import OneEuroFilterTransform
+import threading
+import time
+import queue
+from typing import Any, Callable
+import numpy as np
+
+from .configs import GRIPPER_NAME, JOINT_NAMES
+from .states import (
+ RobotActivityState, ControllerState, TeleopState,
+ RobotState, IKState, CameraState
+)
class RobotActivityState(Enum):
"""Robot activity state enumeration."""
-
ENABLED = "ENABLED"
HOMING = "HOMING"
DISABLED = "DISABLED"
@@ -54,6 +65,10 @@ def __init__(self) -> None:
self.active: bool = False
self.controller_initial_transform: np.ndarray | None = None
self.robot_initial_transform: np.ndarray | None = None
+ # Teleoperation scaling parameters (how much controller motion maps to robot motion)
+ self.translation_scale: float = 1.0
+ self.rotation_scale: float = 1.0
+ self.slow_scaling_mode_enabled: bool = False
class RobotState:
@@ -64,6 +79,7 @@ def __init__(self) -> None:
self._lock = threading.Lock()
self.joint_angles: np.ndarray | None = None
+ self.joint_torques: np.ndarray | None = None
self.end_effector_pose: np.ndarray | None = None
self.current_gripper_open_value: float | None = None
self.target_gripper_open_value: float | None = None
@@ -84,14 +100,14 @@ def __init__(self) -> None:
class CameraState:
- """Camera state - RGB image, depth image, point cloud."""
+ """Camera state - RGB images for one or more cameras."""
def __init__(self) -> None:
"""Initialize CameraState with default values."""
self._lock = threading.Lock()
- self.rgb_image: np.ndarray | None = None
-
+ # Map from camera name -> latest RGB image
+ self.rgb_images: dict[str, np.ndarray] = {}
class DataManager:
"""Main state container coordinating all state groups.
@@ -103,61 +119,93 @@ class DataManager:
Uses separate locks for each state group to reduce contention.
"""
-
def __init__(self) -> None:
- """Initialize DataManager with default values."""
- # State groups with individual locks
+ """Initialize DataManager with background callback processing."""
self._controller_state = ControllerState()
self._teleop_state = TeleopState()
self._robot_state = RobotState()
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 arguments: (stream_name: str, data: Any, timestamp: float)
- self._on_change_callback: Callable[[str, Any, float], None] | None = None
+ # 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, Any, float], None]
+ self, on_change_callback: Callable[[str, dict[str, Any], float], None]
) -> None:
"""Set on change callback (thread-safe)."""
self._on_change_callback = on_change_callback
+ def _queue_callback(self, name: str, payload: dict[str, Any], timestamp: float) -> None:
+ """Helper to push payloads into the execution queue without blocking."""
+ if self._on_change_callback is None:
+ return
+
+ try:
+ # put_nowait drops data into the memory queue instantly (0.0ms blocking)
+ self._callback_queue.put_nowait((name, payload, timestamp))
+ except queue.Full:
+ # Prevents out-of-memory if disk halts completely, without freezing telemetry loops
+ print(f"โ ๏ธ Neuracore background queue full! Dropping log packet: {name}")
+
+ def _callback_worker_loop(self) -> None:
+ """Background thread worker loop dedicated solely to performing slow disk IO updates."""
+ while not self._shutdown_event.is_set() or not self._callback_queue.empty():
+ try:
+ # Wait up to 100ms for a logging event
+ name, payload, timestamp = self._callback_queue.get(timeout=0.1)
+
+ if self._on_change_callback is not None:
+ # Execute Neuracore disk operation safely here on a separate core
+ self._on_change_callback(name, payload, timestamp)
+
+ self._callback_queue.task_done()
+ except queue.Empty:
+ continue
+ except Exception as e:
+ print(f"โ Error in background logging callback: {e}")
+
# ============================================================================
# Camera State Methods
# ============================================================================
- def get_rgb_image(self) -> np.ndarray | None:
- """Get RGB image (thread-safe)."""
+ def get_rgb_image(self, camera_name: str) -> np.ndarray | None:
+ """Get RGB image for a specific camera (thread-safe)."""
with self._camera_state._lock:
- return (
- self._camera_state.rgb_image.copy()
- if self._camera_state.rgb_image is not None
- else None
- )
+ if not self._camera_state.rgb_images:
+ return None
+ img = self._camera_state.rgb_images.get(camera_name)
+ return img.copy() if img is not None else None
- def set_rgb_image(self, image: np.ndarray) -> None:
- """Set RGB image (thread-safe)."""
+ def set_rgb_image(self, image: np.ndarray, camera_name: str) -> None:
+ """Set RGB image for a specific camera (thread-safe and non-blocking)."""
with self._camera_state._lock:
- self._camera_state.rgb_image = image.copy()
+ self._camera_state.rgb_images[camera_name] = image.copy()
+
if self._on_change_callback:
- self._on_change_callback(
- "log_rgb", self._camera_state.rgb_image.copy(), time.time()
- )
+ img_copy = self._camera_state.rgb_images[camera_name].copy()
+ # Queue it instead of executing directly! Camera loop returns immediately.
+ self._queue_callback("log_rgb", {camera_name: img_copy}, time.time())
# ============================================================================
# Controller State Methods
# ============================================================================
def get_controller_data(self) -> tuple[np.ndarray | None, float, float]:
- """Get current controller data (thread-safe).
-
- Returns:
- Tuple of (controller_transform, grip_value, trigger_value)
- """
with self._controller_state._lock:
return (
(
@@ -172,18 +220,6 @@ def get_controller_data(self) -> tuple[np.ndarray | None, float, float]:
def set_controller_data(
self, transform: np.ndarray | None, grip: float, trigger: float
) -> None:
- """Set controller data (thread-safe).
-
- Args:
- transform: np.ndarray | None - 4x4 transformation matrix or None
- grip: float - grip value
- trigger: float - trigger value
-
- Raises:
- ValueError: If the transform is not a 4x4 matrix
- ValueError: If the grip value is not between 0.0 and 1.0
- ValueError: If the trigger value is not between 0.0 and 1.0
- """
if transform is not None and transform.shape != (4, 4):
raise ValueError("Transform must be a 4x4 matrix")
if grip < 0.0 or grip > 1.0:
@@ -197,11 +233,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,
@@ -212,45 +245,28 @@ def set_controller_data(
)
self._controller_state.transform = transform.copy()
else:
- # Update filter parameters if they changed
self._controller_state._filter.update_params(
self._controller_state.min_cutoff,
self._controller_state.beta,
self._controller_state.d_cutoff,
)
-
- # Apply filter
self._controller_state.transform = self._controller_state._filter(
current_time, transform
)
else:
self._controller_state.transform = None
self._controller_state.transform_raw = None
- self._controller_state._filter = (
- None # Reset filter when transform is None
- )
+ self._controller_state._filter = None
def set_controller_filter_params(
self, min_cutoff: float, beta: float, d_cutoff: float
) -> None:
- """Update 1โฌ Filter parameters for controller transform (thread-safe).
-
- Args:
- min_cutoff: Minimum cutoff frequency (stabilizes when holding still)
- beta: Speed coefficient (reduces lag when moving)
- d_cutoff: Cutoff frequency for derivative filtering
- """
with self._controller_state._lock:
self._controller_state.min_cutoff = min_cutoff
self._controller_state.beta = beta
self._controller_state.d_cutoff = d_cutoff
def get_controller_filter_params(self) -> tuple[float, float, float]:
- """Get 1โฌ Filter parameters for controller transform (thread-safe).
-
- Returns:
- Tuple of (min_cutoff, beta, d_cutoff)
- """
with self._controller_state._lock:
return (
self._controller_state.min_cutoff,
@@ -268,13 +284,6 @@ def set_teleop_state(
controller_initial: np.ndarray | None,
robot_initial: np.ndarray | None,
) -> None:
- """Set teleoperation state (thread-safe).
-
- Args:
- active: bool - whether teleop is active
- controller_initial: np.ndarray | None - 4x4 transformation matrix for initial controller transform or None to clear
- robot_initial: np.ndarray | None - 4x4 transformation matrix for initial robot transform or None to clear
- """
with self._teleop_state._lock:
self._teleop_state.active = active
self._teleop_state.controller_initial_transform = (
@@ -284,22 +293,44 @@ def set_teleop_state(
robot_initial.copy() if robot_initial is not None else None
)
+ def set_teleop_scaling(
+ self, translation_scale: float, rotation_scale: float
+ ) -> None:
+ if translation_scale <= 0.0 or rotation_scale <= 0.0:
+ return
+ with self._teleop_state._lock:
+ self._teleop_state.translation_scale = translation_scale
+ self._teleop_state.rotation_scale = rotation_scale
+
+ def get_teleop_scaling(self) -> tuple[float, float]:
+ with self._teleop_state._lock:
+ return (
+ self._teleop_state.translation_scale,
+ self._teleop_state.rotation_scale,
+ )
+
def get_teleop_active(self) -> bool:
- """Get teleoperation active state (thread-safe)."""
with self._teleop_state._lock:
return self._teleop_state.active
+ def set_slow_scaling_mode_enabled(self, enabled: bool) -> None:
+ with self._teleop_state._lock:
+ self._teleop_state.slow_scaling_mode_enabled = enabled
+
+ def toggle_slow_scaling_mode_enabled(self) -> bool:
+ with self._teleop_state._lock:
+ self._teleop_state.slow_scaling_mode_enabled = (
+ not self._teleop_state.slow_scaling_mode_enabled
+ )
+ return self._teleop_state.slow_scaling_mode_enabled
+
+ def get_slow_scaling_mode_enabled(self) -> bool:
+ with self._teleop_state._lock:
+ return self._teleop_state.slow_scaling_mode_enabled
+
def get_initial_robot_controller_transforms(
self,
) -> tuple[np.ndarray | None, np.ndarray | None]:
- """Get initial robot and controller transforms.
-
- These two transforms are captured on rising edge of grip button
- and reset on falling edge of grip button. (thread-safe)
-
- Returns:
- Tuple of (controller_initial_transform, robot_initial_transform)
- """
with self._teleop_state._lock:
return (
(
@@ -319,29 +350,14 @@ def get_initial_robot_controller_transforms(
# ============================================================================
def get_robot_activity_state(self) -> RobotActivityState:
- """Get robot activity state (thread-safe).
-
- Returns:
- RobotActivityState - current robot activity state
- """
with self._robot_state._lock:
return self._robot_state.activity_state
def set_robot_activity_state(self, state: RobotActivityState) -> None:
- """Set robot activity state (thread-safe).
-
- Args:
- state: RobotActivityState - new robot activity state
- """
with self._robot_state._lock:
self._robot_state.activity_state = state
def get_current_joint_angles(self) -> np.ndarray | None:
- """Get current joint angles (thread-safe).
-
- Returns:
- Current joint angles or None if not available
- """
with self._robot_state._lock:
return (
self._robot_state.joint_angles.copy()
@@ -350,26 +366,34 @@ def get_current_joint_angles(self) -> np.ndarray | None:
)
def set_current_joint_angles(self, angles: np.ndarray) -> None:
- """Set current joint angles (thread-safe).
-
- Args:
- angles: np.ndarray - current joint angles
- """
with self._robot_state._lock:
self._robot_state.joint_angles = angles.copy()
if self._on_change_callback:
- self._on_change_callback(
- "log_joint_positions",
- self._robot_state.joint_angles.copy(),
- time.time(),
+ angles = self._robot_state.joint_angles
+ if angles is not None:
+ payload = {
+ jn: float(np.radians(angles[i])) for i, jn in enumerate(JOINT_NAMES)
+ }
+ self._queue_callback("log_joint_positions", payload, time.time())
+
+ def get_current_joint_torques(self) -> np.ndarray | None:
+ with self._robot_state._lock:
+ return (
+ self._robot_state.joint_torques.copy()
+ if self._robot_state.joint_torques is not None
+ else None
)
- def get_current_end_effector_pose(self) -> np.ndarray | None:
- """Get current end effector pose (thread-safe).
+ def set_current_joint_torques(self, torques: np.ndarray) -> None:
+ with self._robot_state._lock:
+ self._robot_state.joint_torques = torques.copy()
+ if self._on_change_callback:
+ torques = self._robot_state.joint_torques
+ if torques is not None:
+ payload = {jn: float(torques[i]) for i, jn in enumerate(JOINT_NAMES)}
+ self._queue_callback("log_joint_torques", payload, time.time())
- Returns:
- Current end effector pose or None if not available
- """
+ def get_current_end_effector_pose(self) -> np.ndarray | None:
with self._robot_state._lock:
return (
self._robot_state.end_effector_pose.copy()
@@ -378,63 +402,34 @@ def get_current_end_effector_pose(self) -> np.ndarray | None:
)
def set_current_end_effector_pose(self, pose: np.ndarray) -> None:
- """Set current end effector pose (thread-safe).
-
- Args:
- pose: np.ndarray - current end effector pose
- """
with self._robot_state._lock:
self._robot_state.end_effector_pose = pose.copy()
def get_current_gripper_open_value(self) -> float | None:
- """Get current gripper open value (thread-safe).
-
- Returns:
- Current gripper open value or None if not available
- """
with self._robot_state._lock:
- return (
- self._robot_state.current_gripper_open_value
- if self._robot_state.current_gripper_open_value is not None
- else None
- )
+ return self._robot_state.current_gripper_open_value
def set_current_gripper_open_value(self, value: float) -> None:
- """Set current gripper open value (thread-safe).
-
- Args:
- value: float - current gripper open value
- """
with self._robot_state._lock:
self._robot_state.current_gripper_open_value = value
if self._on_change_callback:
- self._on_change_callback(
+ self._queue_callback(
"log_parallel_gripper_open_amounts",
- value,
+ {GRIPPER_NAME: value},
time.time(),
)
def get_target_gripper_open_value(self) -> float | None:
- """Get target gripper open value (thread-safe).
-
- Returns:
- Target gripper open value or None if not available
- """
with self._robot_state._lock:
return self._robot_state.target_gripper_open_value
def set_target_gripper_open_value(self, value: float) -> None:
- """Set target gripper open value (thread-safe).
-
- Args:
- value: float - target gripper open value
- """
with self._robot_state._lock:
self._robot_state.target_gripper_open_value = value
if self._on_change_callback:
- self._on_change_callback(
+ self._queue_callback(
"log_parallel_gripper_target_open_amounts",
- self._robot_state.target_gripper_open_value,
+ {GRIPPER_NAME: self._robot_state.target_gripper_open_value},
time.time(),
)
@@ -443,11 +438,6 @@ def set_target_gripper_open_value(self, value: float) -> None:
# ============================================================================
def get_target_joint_angles(self) -> np.ndarray | None:
- """Get current joint configuration (thread-safe).
-
- Returns:
- Current target joint angles or None if not available
- """
with self._ik_state._lock:
return (
self._ik_state.target_joint_angles.copy()
@@ -456,37 +446,25 @@ def get_target_joint_angles(self) -> np.ndarray | None:
)
def set_target_joint_angles(self, angles: np.ndarray) -> None:
- """Set target joint angles (thread-safe).
-
- Args:
- angles: np.ndarray - target joint angles
- """
with self._ik_state._lock:
self._ik_state.target_joint_angles = angles.copy()
if self._on_change_callback:
- self._on_change_callback(
- "log_joint_target_positions",
- self._ik_state.target_joint_angles.copy(),
- time.time(),
- )
+ angles = self._ik_state.target_joint_angles
+ if angles is not None:
+ payload = {
+ jn: float(np.radians(angles[i])) for i, jn in enumerate(JOINT_NAMES)
+ }
+ self._queue_callback(
+ "log_joint_target_positions", payload, time.time()
+ )
def set_target_pose(self, transform: np.ndarray | None) -> None:
- """Set target transform for visualization (thread-safe).
-
- Args:
- transform: np.ndarray | None - 4x4 transformation matrix or None to clear target transform
- """
with self._ik_state._lock:
self._ik_state.target_pose = (
transform.copy() if transform is not None else None
)
def get_target_pose(self) -> np.ndarray | None:
- """Get target transform for visualization (thread-safe).
-
- Returns:
- Target transform or None if target transform is not set
- """
with self._ik_state._lock:
return (
self._ik_state.target_pose.copy()
@@ -495,38 +473,18 @@ def get_target_pose(self) -> np.ndarray | None:
)
def set_ik_solve_time_ms(self, time_ms: float) -> None:
- """Set IK solve time (thread-safe).
-
- Args:
- time_ms: float - IK solve time in milliseconds
- """
with self._ik_state._lock:
self._ik_state.solve_time_ms = time_ms
def set_ik_success(self, success: bool) -> None:
- """Set IK success (thread-safe).
-
- Args:
- success: bool - True if IK was successful, False otherwise
- """
with self._ik_state._lock:
self._ik_state.success = success
def get_ik_solve_time_ms(self) -> float:
- """Get IK solve time (thread-safe).
-
- Returns:
- IK solve time in milliseconds
- """
with self._ik_state._lock:
return self._ik_state.solve_time_ms
def get_ik_success(self) -> bool:
- """Get IK success (thread-safe).
-
- Returns:
- True if IK was successful, False otherwise
- """
with self._ik_state._lock:
return self._ik_state.success
@@ -535,13 +493,9 @@ def get_ik_success(self) -> bool:
# ============================================================================
def request_shutdown(self) -> None:
- """Request shutdown of all threads (lock-free using Event)."""
+ """Request shutdown of all threads."""
self._shutdown_event.set()
def is_shutdown_requested(self) -> bool:
- """Check if shutdown is requested (lock-free using Event).
-
- Returns:
- True if shutdown is requested, False otherwise
- """
- return self._shutdown_event.is_set()
+ """Check if shutdown is requested."""
+ return self._shutdown_event.is_set()
\ No newline at end of file
diff --git a/examples/common/dataset_helpers.py b/examples/common/dataset_helpers.py
new file mode 100644
index 0000000..a0676a0
--- /dev/null
+++ b/examples/common/dataset_helpers.py
@@ -0,0 +1,46 @@
+import neuracore as nc
+from neuracore_types import DataType, CrossEmbodimentDescription
+from neuracore.core.utils.robot_data_spec_utils import merge_cross_embodiment_description
+
+def load_and_sync_dataset(
+ dataset_name: str,
+ frequency: int,
+ input_modalities: list[DataType] | None = None,
+ output_modalities: list[DataType] | None = None,
+ prefetch_videos: bool = False
+):
+ """Loads a Neuracore dataset and synchronizes the specified modalities across embodiments."""
+ print(f"\n๐ Getting dataset '{dataset_name}' from Neuracore...")
+ dataset = nc.get_dataset(dataset_name)
+
+ print("๐ Building cross_embodiment_union for synchronization...")
+ input_cross_embodiment: CrossEmbodimentDescription = {}
+ output_cross_embodiment: CrossEmbodimentDescription = {}
+
+ for robot_id in dataset.robot_ids:
+ full = dataset.get_full_embodiment_description(robot_id)
+ if input_modalities:
+ input_cross_embodiment[robot_id] = {dt: full[dt] for dt in input_modalities if dt in full}
+ if output_modalities:
+ output_cross_embodiment[robot_id] = {dt: full[dt] for dt in output_modalities if dt in full}
+
+ cross_embodiment_union = merge_cross_embodiment_description(
+ input_cross_embodiment, output_cross_embodiment
+ )
+
+ print("๐ Synchronizing dataset...")
+
+ # Dynamically build arguments to avoid passing 0 workers to the ThreadPoolExecutor
+ sync_kwargs = {
+ "frequency": frequency,
+ "cross_embodiment_union": cross_embodiment_union,
+ "prefetch_videos": prefetch_videos
+ }
+
+ if prefetch_videos:
+ sync_kwargs["max_prefetch_workers"] = 2
+
+ synced_dataset = dataset.synchronize(**sync_kwargs)
+
+ print(f" โ Dataset synchronized: {len(synced_dataset)} episodes")
+ return synced_dataset
\ No newline at end of file
diff --git a/examples/common/foot_pedal.py b/examples/common/foot_pedal.py
new file mode 100644
index 0000000..903b552
--- /dev/null
+++ b/examples/common/foot_pedal.py
@@ -0,0 +1,107 @@
+"""Foot pedal abstraction with callback dispatch and key mapping."""
+
+import os
+import sys
+import time
+import traceback
+from typing import Callable
+
+# add the parent directory to the path
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+from common.data_manager import DataManager
+
+
+class FootPedal:
+ """Task-agnostic pedal/keyboard mapping with dynamic button support."""
+
+ def __init__(self, key_map: dict[str, str] | None = None) -> None:
+ """Initialize dynamic button mapping and callback registry."""
+ source = key_map if key_map is not None else {}
+ self._key_map = {name: str(key).strip().lower() for name, key in source.items()}
+ self._callbacks: dict[str, Callable[[], None]] = {}
+
+ @property
+ def key_map(self) -> dict[str, str]:
+ """Return a copy of the current button-to-key map."""
+ return dict(self._key_map)
+
+ def set_key_map(self, key_map: dict[str, str]) -> None:
+ """Set the key map for the foot pedal. Will override the current key map.
+
+ Args:
+ key_map: The key map to set for the foot pedal.
+ """
+ self._key_map = {
+ name: str(key).strip().lower() for name, key in key_map.items()
+ }
+
+ def bind(self, button_name: str, callback: Callable[[], None] | None) -> None:
+ """Bind a callback to a button name.
+
+ Args:
+ button_name: The name of the button to bind the callback to.
+ callback: The callback to bind to the button name.
+
+ NOTE: if callback is None, the callback is removed from the button name.
+ """
+ if callback is None:
+ self._callbacks.pop(button_name, None)
+ return
+ self._callbacks[button_name] = callback
+
+ def get_bound_buttons(self) -> list[str]:
+ """Return button names that currently have callbacks bound."""
+ return list(self._callbacks.keys())
+
+ def _dispatch(self, char: str) -> None:
+ """Fire mapped callback for a pressed key."""
+ normalized = str(char).strip().lower()
+ for button_name, mapped_key in self._key_map.items():
+ if normalized == mapped_key:
+ callback = self._callbacks.get(button_name)
+ if callback:
+ callback()
+
+ def run_loop(self, data_manager: DataManager) -> None:
+ """Block and dispatch pedal events until shutdown is requested."""
+ print("โจ๏ธ Foot pedal listener started.")
+
+ try:
+ from pynput import keyboard
+
+ print("โจ๏ธ Foot pedal listener (pynput) started.")
+
+ def on_press(key: object) -> None:
+ try:
+ char = key.char if hasattr(key, "char") else str(key)
+ self._dispatch(char)
+ except Exception:
+ pass
+
+ with keyboard.Listener(on_press=on_press) as listener:
+ while not data_manager.is_shutdown_requested():
+ if not listener.is_alive():
+ break
+ time.sleep(0.1)
+ listener.stop()
+
+ except Exception as e:
+ print(f"โ Fatal error in foot pedal: {e}")
+ traceback.print_exc()
+ finally:
+ print("โจ๏ธ Foot pedal listener stopped.")
+
+
+if __name__ == "__main__":
+ print("๐ Standalone Foot Pedal Hardware Test")
+ print("--------------------------------------")
+ data_manager = DataManager()
+ pedal = FootPedal({"button_a": "a", "button_b": "b", "button_c": "c"})
+ pedal.bind("button_a", lambda: print("โ ๐ก Pedal A (ENABLE/DISABLE) detected"))
+ pedal.bind("button_b", lambda: print("โ ๐ Pedal B (HOME) detected"))
+ pedal.bind("button_c", lambda: print("โ ๐ด Pedal C (RECORD) detected"))
+
+ try:
+ pedal.run_loop(data_manager)
+ except KeyboardInterrupt:
+ print("\n๐ Test stopped.")
diff --git a/examples/common/policy_actions.py b/examples/common/policy_actions.py
new file mode 100644
index 0000000..057954a
--- /dev/null
+++ b/examples/common/policy_actions.py
@@ -0,0 +1,269 @@
+"""Extracted actions for running and executing policies across different rollout scripts."""
+
+import time
+import threading
+import numpy as np
+from typing import Any, Optional
+
+from common.configs import (
+ JOINT_NAMES, GRIPPER_NAME, MAX_SAFETY_THRESHOLD, MAX_ACTION_ERROR_THRESHOLD,
+ TARGETING_POSE_TIME_THRESHOLD, POLICY_EXECUTION_RATE, CAMERA_NAMES
+)
+from common.data_manager import DataManager, RobotActivityState
+from common.policy_state import PolicyState
+from common.policy_helpers import (
+ convert_predictions_to_horizon, log_robot_state_for_policy
+)
+
+
+def run_policy(
+ data_manager: DataManager, policy: Any, policy_state: PolicyState,
+ visualizer: Optional[Any], input_embodiment_description: dict
+) -> bool:
+ """Handle Run Policy trigger to capture state and get policy prediction."""
+ print("Running policy...")
+
+ # Use our helper to log only what the model expects
+ if not log_robot_state_for_policy(data_manager, input_embodiment_description):
+ print("โ No data available to run policy")
+ return False
+
+ try:
+ start_time = time.time()
+ predictions = policy.predict(timeout=60)
+ inference_time = time.time() - start_time
+ prediction_horizon = convert_predictions_to_horizon(predictions)
+
+ horizon_length = len(next(iter(prediction_horizon.values()))) if prediction_horizon else 0
+ print(f" โ Got policy prediction in {inference_time:.2f}s with horizon length {horizon_length}")
+
+ if visualizer:
+ policy_state.set_execution_ratio(visualizer.get_prediction_ratio())
+
+ policy_state.set_prediction_horizon(prediction_horizon)
+
+ if visualizer:
+ visualizer.update_ghost_robot_visibility(True)
+
+ policy_state.set_ghost_robot_playing(True)
+ policy_state.reset_ghost_action_index()
+ return True
+ except Exception as e:
+ print(f"โ Failed to get policy prediction: {e}")
+ return False
+
+
+def start_policy_execution(data_manager: DataManager, policy_state: PolicyState) -> bool:
+ """Handle Execute Policy trigger to start policy execution."""
+ print("Starting policy execution...")
+ state = data_manager.get_robot_activity_state()
+
+ if state == RobotActivityState.POLICY_CONTROLLED and not policy_state.get_continuous_play_active():
+ print("โ ๏ธ Policy execution already in progress")
+ return False
+ if state == RobotActivityState.DISABLED:
+ print("โ ๏ธ Cannot execute policy: Robot is disabled")
+ return False
+
+ if policy_state.get_prediction_horizon_length() == 0:
+ print("โ ๏ธ No prediction horizon available.")
+ return False
+
+ current_joint_angles = data_manager.get_current_joint_angles()
+ if current_joint_angles is None:
+ return False
+
+ prediction_horizon = policy_state.get_prediction_horizon()
+ first_targets = np.degrees([prediction_horizon[jn][0] for jn in JOINT_NAMES])
+
+ # Safety check: Prevent wild jumps
+ if np.any(np.abs(current_joint_angles - first_targets) > MAX_SAFETY_THRESHOLD):
+ print("โ ๏ธ Cannot execute policy: Robot too far from first predicted action")
+ return False
+
+ policy_state.set_ghost_robot_playing(False)
+ data_manager.set_teleop_state(False, None, None)
+ policy_state.start_policy_execution()
+
+ if policy_state.get_locked_prediction_horizon_length() == 0:
+ policy_state.end_policy_execution()
+ return False
+
+ data_manager.set_robot_activity_state(RobotActivityState.POLICY_CONTROLLED)
+ return True
+
+
+def end_policy_play(data_manager: DataManager, policy_state: PolicyState, visualizer: Optional[Any], status_msg: str) -> None:
+ """End continuous play and update system state."""
+ if policy_state.get_continuous_play_active():
+ policy_state.set_continuous_play_active(False)
+
+ if visualizer:
+ visualizer.set_ghost_robot_color((1.0, 0.65, 0.0, 0.25))
+ visualizer.update_play_policy_button_status(False)
+ visualizer.update_policy_status(status_msg)
+
+ policy_state.end_policy_execution()
+ data_manager.set_robot_activity_state(RobotActivityState.ENABLED)
+ data_manager.set_teleop_state(False, None, None)
+
+
+def continuous_prediction_worker(data_manager, policy, policy_state, visualizer, input_emb, continuous_mode):
+ """Background thread for continuous receding horizon execution."""
+ colors = [(1.0, 0.65, 0.0, 0.25), (0.0, 1.0, 0.0, 0.25), (1.0, 0.0, 0.0, 0.25), (0.0, 0.0, 1.0, 0.25)]
+ c_idx = 0
+
+ print(f"\n๐ Bootstrapping initial trajectory in '{continuous_mode}' mode...")
+ if run_policy(data_manager, policy, policy_state, visualizer, input_emb):
+ start_policy_execution(data_manager, policy_state)
+
+ while policy_state.get_continuous_play_active():
+ if policy_state.get_locked_prediction_horizon_length() == 0:
+ time.sleep(0.01); continue
+
+ if continuous_mode == "pipeline":
+ # Predict next horizon in the background while moving
+ success = run_policy(data_manager, policy, policy_state, visualizer, input_emb)
+ if not success:
+ time.sleep(0.05); continue
+
+ # Wait until current trajectory is almost finished before hot-swapping
+ while policy_state.get_continuous_play_active():
+ rem = policy_state.get_locked_prediction_horizon_length() - policy_state.get_execution_action_index()
+ if rem <= 5 or policy_state.get_locked_prediction_horizon_length() == 0:
+ break
+ time.sleep(0.01)
+
+ elif continuous_mode == "sequential":
+ # Wait for current trajectory to fully finish before querying network
+ while policy_state.get_continuous_play_active():
+ if policy_state.get_execution_action_index() >= policy_state.get_locked_prediction_horizon_length():
+ break
+ time.sleep(0.01)
+
+ if not policy_state.get_continuous_play_active(): break
+
+ success = run_policy(data_manager, policy, policy_state, visualizer, input_emb)
+ if not success:
+ time.sleep(0.05); continue
+
+ if not policy_state.get_continuous_play_active(): break
+
+ policy_state.end_policy_execution()
+ if start_policy_execution(data_manager, policy_state):
+ c_idx = (c_idx + 1) % len(colors)
+ if visualizer: visualizer.set_ghost_robot_color(colors[c_idx])
+ else:
+ time.sleep(0.01)
+
+
+def play_policy(data_manager, policy, policy_state, visualizer, input_emb, continuous_mode="pipeline"):
+ """Toggle for starting/stopping continuous policy mode."""
+ if not policy_state.get_continuous_play_active():
+ print(f"โถ๏ธ Starting {continuous_mode.capitalize()} Mode...")
+ policy_state.set_continuous_play_active(True)
+ if visualizer: visualizer.update_play_policy_button_status(True)
+ threading.Thread(
+ target=continuous_prediction_worker,
+ args=(data_manager, policy, policy_state, visualizer, input_emb, continuous_mode),
+ daemon=True
+ ).start()
+ else:
+ print("โน๏ธ Stopping continuous policy execution...")
+ end_policy_play(data_manager, policy_state, visualizer, "Policy execution stopped")
+
+
+def policy_execution_thread(policy, data_manager, policy_state, robot_controller, visualizer, input_emb):
+ """The thread that continuously reads the locked horizon and sends joint commands."""
+ dt_execution = 1.0 / POLICY_EXECUTION_RATE
+ last_vis_update = 0.0
+
+ while True:
+ start_time = time.time()
+
+ if data_manager.get_robot_activity_state() == RobotActivityState.POLICY_CONTROLLED:
+ locked_horizon = policy_state.get_locked_prediction_horizon()
+ exec_idx = policy_state.get_execution_action_index()
+ locked_len = policy_state.get_locked_prediction_horizon_length()
+
+ if exec_idx < locked_len:
+ current_angles = data_manager.get_current_joint_angles()
+
+ # Check target pose tracking if in pose-execution mode
+ if exec_idx > 0 and current_angles is not None and policy_state.get_execution_mode() == PolicyState.ExecutionMode.TARGETING_POSE:
+ t_start = time.time()
+ while (time.time() - t_start) < TARGETING_POSE_TIME_THRESHOLD:
+ prev_targs = np.degrees([locked_horizon[jn][exec_idx - 1] for jn in JOINT_NAMES])
+ if np.any(np.abs(current_angles - prev_targs) <= MAX_ACTION_ERROR_THRESHOLD):
+ break
+ time.sleep(0.001)
+
+ if all(jn in locked_horizon for jn in JOINT_NAMES):
+ targs_deg = np.degrees([locked_horizon[jn][exec_idx] for jn in JOINT_NAMES])
+ data_manager.set_target_joint_angles(targs_deg)
+ if robot_controller.is_robot_enabled():
+ robot_controller.set_target_joint_angles(targs_deg)
+
+ if GRIPPER_NAME in locked_horizon:
+ robot_controller.set_gripper_open_value(locked_horizon[GRIPPER_NAME][exec_idx])
+
+ policy_state.increment_execution_action_index()
+ if visualizer:
+ visualizer.update_policy_status(f"Executing: {exec_idx + 1}/{locked_len}")
+ else:
+ if not policy_state.get_continuous_play_active():
+ end_policy_play(data_manager, policy_state, visualizer, "Execution completed")
+ elif all(jn in locked_horizon for jn in JOINT_NAMES) and robot_controller.is_robot_enabled():
+ # Hold last position while waiting for background thread to swap horizon
+ robot_controller.set_target_joint_angles(np.degrees([locked_horizon[jn][-1] for jn in JOINT_NAMES]))
+
+ # Throttle visualizer updates to 30Hz
+ if visualizer and (time.time() - last_vis_update >= 1.0 / 30.0):
+ _update_visualization(data_manager, policy_state, visualizer)
+ last_vis_update = time.time()
+
+ time.sleep(max(0, dt_execution - (time.time() - start_time)))
+
+
+def _update_visualization(data_manager: DataManager, policy_state: PolicyState, visualizer: Any) -> None:
+ """Sync the visualizer UI with the DataManager's internal state."""
+ current_joints = data_manager.get_current_joint_angles()
+ if current_joints is not None:
+ visualizer.update_robot_pose(np.radians(current_joints))
+
+ rgb_image = data_manager.get_rgb_image(CAMERA_NAMES[0])
+ if rgb_image is not None:
+ visualizer.update_rgb_image(rgb_image)
+
+ state = data_manager.get_robot_activity_state()
+ if state == RobotActivityState.POLICY_CONTROLLED:
+ visualizer.update_ghost_robot_visibility(True)
+ t_joints = data_manager.get_target_joint_angles()
+ if t_joints is not None: visualizer.update_ghost_robot_pose(np.radians(t_joints))
+ visualizer.set_run_policy_button_disabled(True)
+ visualizer.set_play_policy_button_disabled(False)
+
+ elif state == RobotActivityState.ENABLED and data_manager.get_teleop_active():
+ visualizer.update_ghost_robot_visibility(True)
+ t_joints = data_manager.get_target_joint_angles()
+ if t_joints is not None: visualizer.update_ghost_robot_pose(np.radians(t_joints))
+
+ elif policy_state.get_ghost_robot_playing() and policy_state.get_prediction_horizon_length() > 0:
+ visualizer.set_start_policy_execution_button_disabled(False)
+ visualizer.update_ghost_robot_visibility(True)
+ g_idx = policy_state.get_ghost_action_index()
+ horizon = policy_state.get_prediction_horizon()
+
+ if g_idx < policy_state.get_prediction_horizon_length() and all(jn in horizon for jn in JOINT_NAMES):
+ visualizer.update_ghost_robot_pose(np.array([horizon[jn][g_idx] for jn in JOINT_NAMES]))
+ policy_state.set_ghost_action_index((g_idx + 1) % policy_state.get_prediction_horizon_length())
+ else:
+ policy_state.reset_ghost_action_index()
+ else:
+ visualizer.update_ghost_robot_visibility(False)
+ has_horizon = policy_state.get_prediction_horizon_length() > 0
+ enabled = state == RobotActivityState.ENABLED
+
+ visualizer.set_start_policy_execution_button_disabled(not (enabled and has_horizon))
+ visualizer.set_run_policy_button_disabled(not enabled)
+ visualizer.set_play_policy_button_disabled(not enabled)
\ No newline at end of file
diff --git a/examples/common/policy_helpers.py b/examples/common/policy_helpers.py
new file mode 100644
index 0000000..a969105
--- /dev/null
+++ b/examples/common/policy_helpers.py
@@ -0,0 +1,214 @@
+"""Shared helpers for AgileX Neuracore policy rollout examples."""
+
+from __future__ import annotations
+
+from typing import Any
+
+import neuracore as nc
+import numpy as np
+from neuracore_types import (
+ BatchedJointData,
+ BatchedNCData,
+ BatchedParallelGripperOpenAmountData,
+ DataType,
+ EmbodimentDescription,
+)
+
+from .configs import GRIPPER_NAME, JOINT_NAMES
+
+DEFAULT_ROBOT_NAME = "AgileX PiPER"
+
+
+def embodiment_names_ordered(spec: list[str] | dict[int, str]) -> list[str]:
+ """Ordered channel names from an embodiment entry (list or indexโname map)."""
+ if isinstance(spec, dict):
+ return [spec[i] for i in sorted(spec)]
+ return list(spec)
+
+
+def get_policy_embodiments(
+ policy: Any,
+) -> tuple[EmbodimentDescription, EmbodimentDescription | None]:
+ """Read input/output embodiment specs resolved on the loaded policy."""
+ if hasattr(policy, "_policy"):
+ inner = policy._policy
+ return inner.input_embodiment_description, inner.output_embodiment_description
+ input_emb = getattr(policy, "input_embodiment_description", None)
+ if input_emb is None:
+ raise AttributeError(
+ "Could not read input_embodiment_description from policy; "
+ "use nc.policy(..., robot_name=...) without overriding embodiments."
+ )
+ output_emb = getattr(policy, "output_embodiment_description", None)
+ return input_emb, output_emb
+
+
+def print_policy_embodiments(
+ input_embodiment: EmbodimentDescription,
+ output_embodiment: EmbodimentDescription | None,
+) -> None:
+ """Print resolved policy embodiment channels."""
+ print("\n๐ Policy input embodiment (from model):")
+ for data_type, spec in input_embodiment.items():
+ print(f" {data_type.name}: {embodiment_names_ordered(spec)}")
+ if output_embodiment is not None:
+ print("\n๐ Policy output embodiment (from model):")
+ for data_type, spec in output_embodiment.items():
+ print(f" {data_type.name}: {embodiment_names_ordered(spec)}")
+
+
+def log_robot_state_for_policy(
+ data_manager: Any,
+ input_embodiment_description: EmbodimentDescription,
+) -> bool:
+ """Log only sensor streams the policy expects. Returns True if anything was logged."""
+ logged_any = False
+
+ if DataType.JOINT_POSITIONS in input_embodiment_description:
+ current_joint_angles = data_manager.get_current_joint_angles()
+ if current_joint_angles is not None:
+ joint_angles_rad = np.radians(current_joint_angles)
+ positions_by_name = {
+ jn: float(ang) for jn, ang in zip(JOINT_NAMES, joint_angles_rad)
+ }
+ policy_joint_order = embodiment_names_ordered(
+ input_embodiment_description[DataType.JOINT_POSITIONS]
+ )
+ nc.log_joint_positions(
+ {jn: positions_by_name[jn] for jn in policy_joint_order}
+ )
+ logged_any = True
+
+ if DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS in input_embodiment_description:
+ gripper_open_value = data_manager.get_current_gripper_open_value()
+ if gripper_open_value is not None:
+ for gripper_name in embodiment_names_ordered(
+ input_embodiment_description[DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS]
+ ):
+ nc.log_parallel_gripper_open_amount(gripper_name, gripper_open_value)
+ logged_any = True
+
+ if DataType.RGB_IMAGES in input_embodiment_description:
+ for camera_name in embodiment_names_ordered(
+ input_embodiment_description[DataType.RGB_IMAGES]
+ ):
+ rgb_image = data_manager.get_rgb_image(camera_name)
+ if rgb_image is not None:
+ nc.log_rgb(camera_name, rgb_image)
+ logged_any = True
+
+ return logged_any
+
+
+def log_sync_step_for_policy(
+ step: Any,
+ input_embodiment_description: EmbodimentDescription,
+) -> bool:
+ """Log a synchronized dataset step using only channels the policy expects."""
+ logged_any = False
+
+ if DataType.JOINT_POSITIONS in input_embodiment_description:
+ joint_data = step.data.get(DataType.JOINT_POSITIONS, {})
+ joint_positions_dict = {
+ joint_name: joint_data[joint_name].value
+ for joint_name in embodiment_names_ordered(
+ input_embodiment_description[DataType.JOINT_POSITIONS]
+ )
+ if joint_name in joint_data
+ }
+ if joint_positions_dict:
+ nc.log_joint_positions(joint_positions_dict)
+ logged_any = True
+
+ if DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS in input_embodiment_description:
+ gripper_data = step.data.get(DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS, {})
+ for gripper_name in embodiment_names_ordered(
+ input_embodiment_description[DataType.PARALLEL_GRIPPER_OPEN_AMOUNTS]
+ ):
+ if gripper_name in gripper_data:
+ nc.log_parallel_gripper_open_amount(
+ gripper_name, gripper_data[gripper_name].open_amount
+ )
+ logged_any = True
+
+ if DataType.RGB_IMAGES in input_embodiment_description:
+ rgb_data = step.data.get(DataType.RGB_IMAGES, {})
+ for camera_name in embodiment_names_ordered(
+ input_embodiment_description[DataType.RGB_IMAGES]
+ ):
+ if camera_name in rgb_data:
+ nc.log_rgb(camera_name, np.array(rgb_data[camera_name].frame))
+ logged_any = True
+
+ return logged_any
+
+
+def convert_predictions_to_horizon(
+ predictions: dict[DataType, dict[str, BatchedNCData]],
+) -> dict[str, list[float]]:
+ """Convert policy predictions to a per-channel horizon dict (model-driven keys)."""
+ horizon: dict[str, list[float]] = {}
+
+ if DataType.JOINT_TARGET_POSITIONS in predictions:
+ for joint_name, batched in predictions[DataType.JOINT_TARGET_POSITIONS].items():
+ if isinstance(batched, BatchedJointData):
+ horizon[joint_name] = batched.value[0, :, 0].cpu().numpy().tolist()
+
+ if DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS in predictions:
+ for gripper_name, batched in predictions[
+ DataType.PARALLEL_GRIPPER_TARGET_OPEN_AMOUNTS
+ ].items():
+ if isinstance(batched, BatchedParallelGripperOpenAmountData):
+ horizon[gripper_name] = (
+ batched.open_amount[0, :, 0].cpu().numpy().tolist()
+ )
+
+ return horizon
+
+
+def horizon_length(horizon: dict[str, list[float]]) -> int:
+ """Number of steps in a prediction horizon dict."""
+ if not horizon:
+ return 0
+ return len(next(iter(horizon.values())))
+
+
+def arm_joint_names_in_horizon(horizon: dict[str, list[float]]) -> list[str]:
+ """Body joint names present in a horizon (excludes gripper channels)."""
+ return [name for name in JOINT_NAMES if name in horizon]
+
+
+def joint_targets_deg_at_index(
+ horizon: dict[str, list[float]], index: int
+) -> np.ndarray | None:
+ """Arm joint targets in degrees at horizon index (Piper JOINT_NAMES order)."""
+ if not all(jn in horizon for jn in JOINT_NAMES):
+ return None
+ if any(index >= len(horizon[jn]) for jn in JOINT_NAMES):
+ return None
+ rad = np.array([horizon[jn][index] for jn in JOINT_NAMES], dtype=np.float64)
+ return np.degrees(rad)
+
+
+def gripper_open_at_index(
+ horizon: dict[str, list[float]],
+ index: int,
+ gripper_names: list[str] | None = None,
+) -> float | None:
+ """Gripper open amount in [0, 1] from the first matching horizon channel."""
+ names = gripper_names or [GRIPPER_NAME]
+ for gripper_name in names:
+ if gripper_name in horizon and index < len(horizon[gripper_name]):
+ return float(np.clip(horizon[gripper_name][index], 0.0, 1.0))
+ return None
+
+
+def urdf_cfg_from_horizon(
+ horizon: dict[str, list[float]], index: int
+) -> np.ndarray | None:
+ """Joint configuration in radians for Viser URDF (JOINT_NAMES order)."""
+ if not all(jn in horizon for jn in JOINT_NAMES):
+ return None
+ if any(index >= len(horizon[jn]) for jn in JOINT_NAMES):
+ return None
+ return np.array([float(horizon[jn][index]) for jn in JOINT_NAMES], dtype=np.float64)
diff --git a/examples/common/policy_state.py b/examples/common/policy_state.py
index 42e90b6..8695aa1 100644
--- a/examples/common/policy_state.py
+++ b/examples/common/policy_state.py
@@ -20,7 +20,7 @@ def __init__(self) -> None:
# Prediction horizon stored as dict[str, list[float]] where keys are joint/gripper names
self._prediction_horizon: dict[str, list[float]] = {}
self._prediction_horizon_lock = threading.Lock()
- self._execution_ratio: float = 1.0
+ self._execution_ratio: float = 0.5 # Default to executing 50% of the predicted horizon, TODO: we need to set it in
self._policy_rgb_image_input: np.ndarray | None = None
self._policy_rgb_image_input_lock = threading.Lock()
@@ -90,14 +90,7 @@ def get_policy_rgb_image_input(self) -> np.ndarray | None:
)
def set_policy_rgb_image_input(self, image: np.ndarray) -> None:
- """Set policy RGB image (thread-safe).
-
- Raises:
- RuntimeError: If policy inputs are locked (during execution).
- """
- with self._execution_lock:
- if self._policy_inputs_locked:
- raise RuntimeError("Policy inputs are locked during execution")
+ """Set policy RGB image (thread-safe)."""
with self._policy_rgb_image_input_lock:
self._policy_rgb_image_input = image.copy() if image is not None else None
@@ -111,14 +104,7 @@ def get_policy_state_input(self) -> np.ndarray | None:
)
def set_policy_state_input(self, input: np.ndarray) -> None:
- """Set policy state input (thread-safe).
-
- Raises:
- RuntimeError: If policy inputs are locked (during execution).
- """
- with self._execution_lock:
- if self._policy_inputs_locked:
- raise RuntimeError("Policy inputs are locked during execution")
+ """Set policy state input (thread-safe)."""
with self._policy_state_input_lock:
self._policy_state_input = input.copy() if input is not None else None
diff --git a/examples/common/robot_visualizer.py b/examples/common/robot_visualizer.py
index e8eb548..137636c 100644
--- a/examples/common/robot_visualizer.py
+++ b/examples/common/robot_visualizer.py
@@ -1,858 +1,29 @@
#!/usr/bin/env python3
"""Shared robot visualizer for Piper robot demos.
-
-This module provides a clean interface for visualizing robot state using Viser,
-encapsulating all the repeated setup, GUI controls, and update logic.
+This module acts as a facade, delegating 3D rendering to RobotVisualizerCore
+and 2D UI elements to RobotVisualizerGUI.
"""
-from typing import Any, Callable
-
-import numpy as np
-import viser
-import yourdfpy
-from scipy.spatial.transform import Rotation
-from viser.extras import ViserUrdf
-
+from .visualizer_core import RobotVisualizerCore
+from .visualizer_gui import RobotVisualizerGUI
class RobotVisualizer:
- """Shared visualizer for robot demos.
-
- Encapsulates viser server setup, GUI controls, and update logic.
- """
+ """Shared visualizer facade for robot demos."""
def __init__(self, urdf_path: str) -> None:
- """Initialize the visualizer.
-
- Args:
- urdf_path: Path to URDF file for robot visualization
- """
- # Initialize viser server
- self._server = viser.ViserServer()
- self._server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1)
-
- # Load URDF for visualization
- urdf = yourdfpy.URDF.load(urdf_path)
- self._urdf_vis = ViserUrdf(self._server, urdf, root_node_name="/robot_actual")
-
- ghost_urdf = yourdfpy.URDF.load(urdf_path)
- # Ghost robot with semi-transparent blue color to make it visually distinct
- self._ghost_robot_urdf = ViserUrdf(
- self._server,
- ghost_urdf,
- root_node_name="/robot_ghost",
- mesh_color_override=(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
-
- # 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_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
-
- def update_play_policy_button_status(self, active: bool) -> None:
- """Update play policy button label based on active state.
-
- Args:
- active: Whether continuous play is currently active
- """
- if self._play_policy_button is not None:
- self._play_policy_button.label = "Stop Policy" if active else "Play Policy"
-
- def stop(self) -> None:
- """Stop the visualizer server."""
- self._server.stop()
+ """Initialize the visualizer components."""
+ self._core = RobotVisualizerCore(urdf_path)
+ self._gui = RobotVisualizerGUI(self._core.server)
+
+ def __getattr__(self, name: str):
+ """
+ Dynamically delegates method calls to the Core or GUI modules.
+ If a top-level script calls `visualizer.add_basic_controls()`,
+ this routes it to `_gui.add_basic_controls()`.
+ """
+ if hasattr(self._core, name):
+ return getattr(self._core, name)
+ if hasattr(self._gui, name):
+ return getattr(self._gui, name)
+
+ raise AttributeError(f"'RobotVisualizer' object has no attribute '{name}'")
diff --git a/examples/common/shared_actions.py b/examples/common/shared_actions.py
new file mode 100644
index 0000000..9526427
--- /dev/null
+++ b/examples/common/shared_actions.py
@@ -0,0 +1,96 @@
+from common.data_manager import DataManager, RobotActivityState
+from piper_controller import PiperController
+import subprocess
+import neuracore as nc
+import numpy as np
+import traceback
+from typing import Any
+from common.configs import JOINT_NAMES, GRIPPER_LOGGING_NAME
+
+def toggle_robot_enabled(data_manager: DataManager, robot_controller: PiperController, visualizer=None):
+ """Safely toggles the robot between ENABLED and DISABLED states."""
+ state = data_manager.get_robot_activity_state()
+
+ if state == RobotActivityState.ENABLED:
+ data_manager.set_robot_activity_state(RobotActivityState.DISABLED)
+ if robot_controller:
+ robot_controller.graceful_stop()
+ data_manager.set_teleop_state(False, None, None)
+ if visualizer:
+ visualizer.update_toggle_robot_enabled_status(False)
+ print("โ ๐ด Robot disabled")
+
+ elif state in (RobotActivityState.DISABLED, RobotActivityState.HOMING):
+ if not robot_controller:
+ data_manager.set_robot_activity_state(RobotActivityState.ENABLED)
+ print("โ ๐ข Robot enabled (Headless)")
+ return
+
+ if robot_controller.resume_robot():
+ data_manager.set_robot_activity_state(RobotActivityState.ENABLED)
+ if visualizer:
+ visualizer.update_toggle_robot_enabled_status(True)
+ print("โ ๐ข Robot enabled")
+ else:
+ print("โ Failed to enable robot")
+
+
+def move_robot_home(data_manager: DataManager, robot_controller: PiperController):
+ """Safely commands the robot to return to its home position."""
+ state = data_manager.get_robot_activity_state()
+
+ if state == RobotActivityState.ENABLED:
+ print("๐ Moving to home position...")
+ data_manager.set_robot_activity_state(RobotActivityState.HOMING)
+ data_manager.set_teleop_state(False, None, None)
+
+ if robot_controller:
+ if not robot_controller.move_to_home():
+ print("โ Failed to initiate home move")
+ data_manager.set_robot_activity_state(RobotActivityState.ENABLED)
+ else:
+ print("โ ๐ Robot homed (Headless)")
+ data_manager.set_robot_activity_state(RobotActivityState.ENABLED)
+ else:
+ print("โ ๏ธ Robot is not enabled, cannot go home")
+
+
+
+
+def toggle_recording(play_audio: bool = False) -> None:
+ """Safely start or stop a Neuracore data recording session."""
+ if not nc.is_recording():
+ try:
+ nc.start_recording()
+ print("โ ๐ด Recording started")
+ if play_audio:
+ subprocess.Popen(["play", "-q", "-n", "synth", "0.3", "sine", "880"], stderr=subprocess.DEVNULL)
+ except Exception as e:
+ print(f"โ Failed to start recording: {e}")
+ else:
+ try:
+ nc.stop_recording()
+ print("โ โน๏ธ Recording stopped")
+ if play_audio:
+ subprocess.Popen(["play", "-q", "-n", "synth", "0.3", "sine", "440"], stderr=subprocess.DEVNULL)
+ except Exception as e:
+ print(f"โ Failed to stop recording: {e}")
+
+def neuracore_logging_callback(name: str, payload: dict[str, Any], timestamp: float) -> None:
+ """Unified callback to map DataManager state changes to Neuracore log streams."""
+ try:
+ if name == "log_joint_positions":
+ nc.log_joint_positions(payload, timestamp=timestamp)
+ elif name == "log_joint_torques":
+ nc.log_joint_torques(payload, timestamp=timestamp)
+ elif name == "log_joint_target_positions":
+ nc.log_joint_target_positions(payload, timestamp=timestamp)
+ elif name == "log_parallel_gripper_open_amounts":
+ nc.log_parallel_gripper_open_amounts(payload, timestamp=timestamp)
+ elif name == "log_parallel_gripper_target_open_amounts":
+ nc.log_parallel_gripper_target_open_amounts(payload, timestamp=timestamp)
+ elif name == "log_rgb":
+ camera_name = next(iter(payload))
+ nc.log_rgb(camera_name, payload[camera_name], timestamp=timestamp)
+ except Exception as e:
+ print(f"โ ๏ธ Logging failed for {name}: {e}")
\ No newline at end of file
diff --git a/examples/common/states.py b/examples/common/states.py
new file mode 100644
index 0000000..0901703
--- /dev/null
+++ b/examples/common/states.py
@@ -0,0 +1,61 @@
+import threading
+from enum import Enum
+import numpy as np
+from common.one_euro_filter import OneEuroFilterTransform
+
+class RobotActivityState(Enum):
+ """Robot activity state enumeration."""
+ ENABLED = "ENABLED"
+ HOMING = "HOMING"
+ DISABLED = "DISABLED"
+ POLICY_CONTROLLED = "POLICY_CONTROLLED"
+
+class ControllerState:
+ """Controller input state - Quest Reader writes, IK/Joint reads."""
+ def __init__(self) -> None:
+ self._lock = threading.Lock()
+ self.min_cutoff: float = 1.0
+ self.beta: float = 0.0
+ self.d_cutoff: float = 1.0
+ self.transform_raw: np.ndarray | None = None
+ self.transform: np.ndarray | None = None
+ self.grip_value: float = 0.0
+ self.trigger_value: float = 0.0
+ self._filter: OneEuroFilterTransform | None = None
+
+class TeleopState:
+ """Teleop activation state - manages teleop start/stop."""
+ def __init__(self) -> None:
+ self._lock = threading.Lock()
+ self.active: bool = False
+ self.controller_initial_transform: np.ndarray | None = None
+ self.robot_initial_transform: np.ndarray | None = None
+ self.translation_scale: float = 1.0
+ self.rotation_scale: float = 1.0
+ self.slow_scaling_mode_enabled: bool = False
+
+class RobotState:
+ """Current robot state - joint angles, end effector pose, activity state."""
+ def __init__(self) -> None:
+ self._lock = threading.Lock()
+ self.joint_angles: np.ndarray | None = None
+ self.joint_torques: np.ndarray | None = None
+ self.end_effector_pose: np.ndarray | None = None
+ self.current_gripper_open_value: float | None = None
+ self.target_gripper_open_value: float | None = None
+ self.activity_state: RobotActivityState = RobotActivityState.DISABLED
+
+class IKState:
+ """IK solution state - target joint angles, pose, metrics."""
+ def __init__(self) -> None:
+ self._lock = threading.Lock()
+ self.target_joint_angles: np.ndarray | None = None
+ self.target_pose: np.ndarray | None = None
+ self.solve_time_ms: float = 0.0
+ self.success: bool = True
+
+class CameraState:
+ """Camera state - RGB images for one or more cameras."""
+ def __init__(self) -> None:
+ self._lock = threading.Lock()
+ self.rgb_images: dict[str, np.ndarray] = {}
\ No newline at end of file
diff --git a/examples/common/system_bootstrap.py b/examples/common/system_bootstrap.py
new file mode 100644
index 0000000..8949ec3
--- /dev/null
+++ b/examples/common/system_bootstrap.py
@@ -0,0 +1,89 @@
+import threading
+import numpy as np
+from typing import Tuple, List, Optional
+
+from common.configs import (
+ ROBOT_RATE, NEUTRAL_JOINT_ANGLES, NEUTRAL_END_EFFECTOR_POSE,
+ URDF_PATH, GRIPPER_FRAME_NAME, SOLVER_NAME, IK_SOLVER_RATE
+)
+from common.data_manager import DataManager
+from piper_controller import PiperController
+from pink_ik_solver import PinkIKSolver
+from common.threads.joint_state import joint_state_thread
+from common.threads.ik_solver import ik_solver_thread
+from common.threads.realsense_camera import camera_thread
+
+def bootstrap_robot_system(
+ config: dict,
+ start_ik: bool = True,
+ start_camera: bool = True
+) -> Tuple[DataManager, PiperController, Optional[PinkIKSolver], List[threading.Thread]]:
+
+ # Extract config sections safely
+ filt_p = config.get("filter_parameters", {})
+ tele_p = config.get("teleop_parameters", {})
+ 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
\ No newline at end of file
diff --git a/examples/common/threads/__init__.py b/examples/common/threads/__init__.py
new file mode 100644
index 0000000..d7d934f
--- /dev/null
+++ b/examples/common/threads/__init__.py
@@ -0,0 +1 @@
+"""Shared thread functions for teleoperation examples."""
diff --git a/examples/common/threads/camera.py b/examples/common/threads/camera.py
deleted file mode 100644
index 8a7b6c6..0000000
--- a/examples/common/threads/camera.py
+++ /dev/null
@@ -1,68 +0,0 @@
-"""Camera thread - captures RGB images from RealSense."""
-
-import time
-import traceback
-
-import numpy as np
-import pyrealsense2 as rs
-from common.configs import CAMERA_FRAME_STREAMING_RATE
-from common.data_manager import DataManager
-
-
-def camera_thread(data_manager: DataManager) -> None:
- """Camera thread - captures RGB images from RealSense."""
- print("๐ท Camera thread started")
-
- dt: float = 1.0 / CAMERA_FRAME_STREAMING_RATE
- pipeline: rs.pipeline | None = None
-
- try:
- # Configure RealSense pipeline
- pipeline = rs.pipeline()
- config = rs.config()
-
- config.enable_stream(
- rs.stream.color,
- 640,
- 480,
- rs.format.rgb8,
- int(CAMERA_FRAME_STREAMING_RATE),
- )
-
- print(f"Starting RealSense pipeline at {CAMERA_FRAME_STREAMING_RATE} Hz...")
- pipeline.start(config)
-
- while not data_manager.is_shutdown_requested():
- iteration_start = time.time()
-
- try:
- frames = pipeline.wait_for_frames(timeout_ms=500)
- except Exception as e:
- print(f"โ ๏ธ RealSense wait for frames error: {e}")
- continue
-
- color_frame = frames.get_color_frame()
-
- 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)
-
- # Sleep to maintain loop rate
- elapsed = time.time() - iteration_start
- sleep_time = dt - elapsed
- if sleep_time > 0:
- time.sleep(sleep_time)
-
- except Exception as e:
- print(f"โ Camera thread error: {e}")
- traceback.print_exc()
- data_manager.request_shutdown()
- finally:
- if pipeline is not None:
- try:
- pipeline.stop()
- print(" โ RealSense pipeline stopped")
- except Exception as e:
- print(f"โ ๏ธ Error stopping pipeline: {e}")
- print("๐ท Camera thread stopped")
diff --git a/examples/common/threads/camera_usb.py b/examples/common/threads/camera_usb.py
new file mode 100644
index 0000000..329c11e
--- /dev/null
+++ b/examples/common/threads/camera_usb.py
@@ -0,0 +1,75 @@
+"""Camera thread - captures RGB images from a USB webcam (OpenCV)."""
+
+import time
+import traceback
+
+import cv2
+from common.configs import (
+ CAMERA_DEVICE_INDEX,
+ CAMERA_FRAME_STREAMING_RATE,
+ CAMERA_HEIGHT,
+ CAMERA_NAMES,
+ CAMERA_WIDTH,
+)
+from common.data_manager import DataManager
+
+
+def camera_thread(data_manager: DataManager) -> None:
+ """Camera thread - captures RGB images from a USB webcam."""
+ print("๐ท Camera thread started (USB webcam)")
+
+ dt: float = 1.0 / CAMERA_FRAME_STREAMING_RATE
+ cap: cv2.VideoCapture | None = None
+ camera_name = CAMERA_NAMES[1]
+
+ try:
+ cap = cv2.VideoCapture(CAMERA_DEVICE_INDEX)
+ if not cap.isOpened():
+ print(
+ f"โ Could not open USB webcam (device index {CAMERA_DEVICE_INDEX}). "
+ "Check connection and permissions."
+ )
+ data_manager.request_shutdown()
+ return
+
+ cap.set(cv2.CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH)
+ cap.set(cv2.CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT)
+ cap.set(cv2.CAP_PROP_FPS, CAMERA_FRAME_STREAMING_RATE)
+
+ # Read back actual resolution (some webcams don't support requested size)
+ actual_w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
+ actual_h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
+ print(
+ f" Webcam opened: {actual_w}x{actual_h} @ ~{CAMERA_FRAME_STREAMING_RATE} Hz"
+ )
+
+ while not data_manager.is_shutdown_requested():
+ iteration_start = time.time()
+
+ ret, frame = cap.read()
+ if not ret or frame is None:
+ print("โ ๏ธ Webcam read failed, skipping frame")
+ time.sleep(dt)
+ continue
+
+ # OpenCV returns BGR; convert to RGB for consistency with pipeline
+ rgb_image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+ # Camera may be mounted upside down; rotate 180ยฐ to keep images upright
+ rgb_image = cv2.rotate(rgb_image, cv2.ROTATE_180)
+ # Treat USB webcam as wrist camera
+ data_manager.set_rgb_image(rgb_image, camera_name)
+
+ elapsed = time.time() - iteration_start
+ sleep_time = dt - elapsed
+ if sleep_time > 0:
+ time.sleep(sleep_time)
+
+ except Exception as e:
+ print(f"โ Camera thread error: {e}")
+ traceback.print_exc()
+ data_manager.request_shutdown()
+ finally:
+ if cap is not None:
+ cap.release()
+ print(" โ USB webcam released")
+ print("๐ท Camera thread stopped")
diff --git a/examples/common/threads/ik_solver.py b/examples/common/threads/ik_solver.py
index e15af40..e43a33e 100644
--- a/examples/common/threads/ik_solver.py
+++ b/examples/common/threads/ik_solver.py
@@ -9,7 +9,7 @@
# Add parent directory to path to import pink_ik_solver
sys.path.insert(0, str(Path(__file__).parent.parent))
-from common.configs import IK_SOLVER_RATE, ROTATION_SCALE, TRANSLATION_SCALE
+from common.configs import IK_SOLVER_RATE
from common.data_manager import DataManager, RobotActivityState
from common.utils import scale_and_add_delta_transform
@@ -29,20 +29,6 @@ def ik_solver_thread(data_manager: DataManager, ik_solver: PinkIKSolver) -> None
# Get current robot joint angles from state
current_joint_angles = data_manager.get_current_joint_angles()
- current_ik_joint_angles = np.degrees(ik_solver.get_current_configuration())
-
- # Sync IK solver with actual joint angles if close enough
- if (
- current_joint_angles is not None
- and current_ik_joint_angles is not None
- and np.all(
- np.abs(current_joint_angles - current_ik_joint_angles)
- <= DIVERGENCE_TOLERANCE
- )
- ):
- ik_solver.set_configuration_no_task_update(
- np.radians(current_joint_angles)
- )
# Get current end effector pose from IK solver and set in state
ik_ee_pose = ik_solver.get_current_end_effector_pose()
@@ -53,6 +39,26 @@ def ik_solver_thread(data_manager: DataManager, ik_solver: PinkIKSolver) -> None
controller_transform, _, _ = data_manager.get_controller_data()
teleop_active = data_manager.get_teleop_active()
+ # Keep IK anchored to the real robot whenever teleop is not actively solving.
+ # This avoids stale IK state after manual joint commands (e.g., Button Y toggle).
+ if current_joint_angles is not None:
+ if not teleop_active:
+ ik_solver.set_configuration_no_task_update(
+ np.radians(current_joint_angles)
+ )
+ else:
+ current_ik_joint_angles = np.degrees(
+ ik_solver.get_current_configuration()
+ )
+ # During active teleop, only hard-sync when IK and hardware are already close.
+ if current_ik_joint_angles is not None and np.all(
+ np.abs(current_joint_angles - current_ik_joint_angles)
+ <= DIVERGENCE_TOLERANCE
+ ):
+ ik_solver.set_configuration_no_task_update(
+ np.radians(current_joint_angles)
+ )
+
# Skip teleop-based IK if in POLICY_CONTROLLED state
# NOTE: During policy execution, the policy execution thread manages target joint angles
# We only update IK solver configuration to keep it in sync, but don't override targets
@@ -83,11 +89,14 @@ def ik_solver_thread(data_manager: DataManager, ik_solver: PinkIKSolver) -> None
controller_transform[:3, :3] @ controller_initial[:3, :3].T
)
+ # Get current teleop scaling factors (from GUI via DataManager)
+ translation_scale, rotation_scale = data_manager.get_teleop_scaling()
+
T_robot_target = scale_and_add_delta_transform(
delta_position,
delta_orientation,
- TRANSLATION_SCALE,
- ROTATION_SCALE,
+ translation_scale,
+ rotation_scale,
robot_initial,
)
diff --git a/examples/common/threads/joint_state.py b/examples/common/threads/joint_state.py
index b39a50a..087f0c1 100644
--- a/examples/common/threads/joint_state.py
+++ b/examples/common/threads/joint_state.py
@@ -29,6 +29,11 @@ def joint_state_thread(
if current_joint_angles is not None:
data_manager.set_current_joint_angles(current_joint_angles)
+ # Use measured joint currents as torque proxy for NeuraCore logging
+ current_joint_currents = robot_controller.get_current_joint_currents()
+ if current_joint_currents is not None:
+ data_manager.set_current_joint_torques(current_joint_currents)
+
# Get current gripper open value and set in state
gripper_open_value = robot_controller.get_current_gripper_open_value()
if gripper_open_value is not None:
@@ -45,17 +50,21 @@ def joint_state_thread(
print("โ Robot reached home position and is re-enabled")
elif robot_activity_state == RobotActivityState.ENABLED:
- if (
- target_joint_angles is not None
- and trigger_value is not None
- and data_manager.get_teleop_active()
- ):
+ if target_joint_angles is not None and data_manager.get_teleop_active():
robot_controller.set_target_joint_angles(target_joint_angles)
- target_gripper_open_value = 1.0 - trigger_value
+
+ if data_manager.get_teleop_active():
+ target_gripper_open_value = max(0.0, min(1.0, 1.0 - trigger_value))
+ robot_controller.set_gripper_open_value(target_gripper_open_value)
+ elif gripper_open_value is not None:
+ target_gripper_open_value = gripper_open_value
+ else:
+ target_gripper_open_value = None
+
+ if target_gripper_open_value is not None:
data_manager.set_target_gripper_open_value(
target_gripper_open_value
)
- robot_controller.set_gripper_open_value(target_gripper_open_value)
# Sleep to maintain streaming rate
elapsed = time.time() - iteration_start
diff --git a/examples/common/threads/realsense_camera.py b/examples/common/threads/realsense_camera.py
new file mode 100644
index 0000000..8573eae
--- /dev/null
+++ b/examples/common/threads/realsense_camera.py
@@ -0,0 +1,120 @@
+"""Camera thread - captures RGB images from RealSense with drop detection."""
+
+import time
+import traceback
+from collections import deque
+import cv2
+
+import numpy as np
+import pyrealsense2 as rs
+from common.configs import CAMERA_FRAME_STREAMING_RATE, CAMERA_NAMES
+from common.data_manager import DataManager
+
+
+def camera_thread(data_manager: DataManager) -> None:
+ """Camera thread - captures RGB images from RealSense and monitors health."""
+ print("๐ท Camera thread started")
+
+ camera_name = CAMERA_NAMES[0]
+ pipeline: rs.pipeline | None = None
+
+ # Diagnostic variables
+ last_frame_number = None
+ total_dropped_frames = 0
+ fps_timer = time.time()
+ frame_count = 0
+ # Store the last 100 frame intervals to check for extreme jitter
+ intervals = deque(maxlen=100)
+ last_frame_time = time.time()
+
+ try:
+ # Configure RealSense pipeline
+ pipeline = rs.pipeline()
+ config = rs.config()
+
+ config.enable_stream(
+ rs.stream.color,
+ 640,
+ 480,
+ rs.format.rgb8,
+ int(CAMERA_FRAME_STREAMING_RATE),
+ )
+
+ print(f"Starting RealSense pipeline at {CAMERA_FRAME_STREAMING_RATE} Hz...")
+ pipeline.start(config)
+
+ while not data_manager.is_shutdown_requested():
+ try:
+ # wait_for_frames naturally blocks at the target framerate (e.g., 60Hz)
+ frames = pipeline.wait_for_frames(timeout_ms=500)
+ except Exception as e:
+ print(f"โ ๏ธ RealSense wait for frames error (Timeout?): {e}")
+ continue
+
+ color_frame = frames.get_color_frame()
+ if not color_frame:
+ continue
+
+ current_time = time.time()
+ intervals.append(current_time - last_frame_time)
+ last_frame_time = current_time
+
+ # ---------------------------------------------------------
+ # DIAGNOSTICS: Check for dropped frames via hardware ID
+ # ---------------------------------------------------------
+ current_frame_number = color_frame.get_frame_number()
+
+ if last_frame_number is not None:
+ # If frame numbers are not sequential, we missed something in software
+ drops = (current_frame_number - last_frame_number) - 1
+ if drops > 0:
+ total_dropped_frames += drops
+ print(f"โ ๏ธ DROPPED {drops} FRAME(S)! (Hardware ID: {current_frame_number}) | Total dropped: {total_dropped_frames}")
+
+ last_frame_number = current_frame_number
+
+ # ---------------------------------------------------------
+ # DIAGNOSTICS: Calculate software-side FPS and Jitter
+ # ---------------------------------------------------------
+ frame_count += 1
+ if current_time - fps_timer >= 5.0: # Report every 5 seconds
+ effective_fps = frame_count / (current_time - fps_timer)
+
+ # Calculate jitter (max variance between frame arrivals)
+ max_interval = max(intervals) * 1000 # in ms
+ min_interval = min(intervals) * 1000 # in ms
+
+ print(f"๐ Camera Health: {effective_fps:.1f} FPS | "
+ f"Jitter: {min_interval:.1f}ms - {max_interval:.1f}ms | "
+ f"Total Drops: {total_dropped_frames}")
+
+ # Reset counters for the next 5-second window
+ fps_timer = current_time
+ frame_count = 0
+
+ # ---------------------------------------------------------
+ # IMAGE PROCESSING
+ # ---------------------------------------------------------
+ # color_image = np.asanyarray(color_frame.get_data())
+ # color_image = np.rot90(color_image, k=3) # Rotate 270 degrees
+ # data_manager.set_rgb_image(color_image, camera_name)
+
+ color_image = np.asanyarray(color_frame.get_data())
+ color_image = np.rot90(color_image, k=3) # Rotate 270 degrees
+ data_manager.set_rgb_image(color_image, camera_name)
+
+ # Notice: The time.sleep() has been completely removed.
+ # wait_for_frames() manages the loop pace perfectly.
+
+ except Exception as e:
+ print(f"โ Camera thread error: {e}")
+ traceback.print_exc()
+ data_manager.request_shutdown()
+ finally:
+ if pipeline is not None:
+ try:
+ pipeline.stop()
+ print(" โ RealSense pipeline stopped")
+ except Exception as e:
+ print(f"โ ๏ธ Error stopping pipeline: {e}")
+ print("๐ท Camera thread stopped")
\ No newline at end of file
diff --git a/examples/common/visualizer_core.py b/examples/common/visualizer_core.py
new file mode 100644
index 0000000..0cffe26
--- /dev/null
+++ b/examples/common/visualizer_core.py
@@ -0,0 +1,92 @@
+import numpy as np
+import viser
+import yourdfpy
+from scipy.spatial.transform import Rotation
+from viser.extras import ViserUrdf
+
+class RobotVisualizerCore:
+ """Handles the 3D rendering of the robot, ghost robot, and target frames."""
+
+ def __init__(self, urdf_path: str) -> None:
+ self.server = viser.ViserServer()
+ self.server.scene.add_grid("/ground", width=2, height=2, cell_size=0.1)
+
+ # Load actual robot URDF
+ urdf = yourdfpy.URDF.load(urdf_path)
+ self.urdf_vis = ViserUrdf(self.server, urdf, root_node_name="/robot_actual")
+
+ # Load ghost robot URDF
+ ghost_urdf = yourdfpy.URDF.load(urdf_path)
+ self.ghost_robot_urdf = ViserUrdf(
+ self.server,
+ ghost_urdf,
+ root_node_name="/robot_ghost",
+ mesh_color_override=(1.0, 0.65, 0.0, 0.25),
+ )
+
+ self.controller_handle = None
+ self.target_frame_handle = None
+ self.rgb_image_handle = None
+
+ def add_controller_visualization(self) -> None:
+ self.controller_handle = self.server.scene.add_transform_controls(
+ "/controller", scale=0.15, position=(0, 0, 0), wxyz=(1, 0, 0, 0)
+ )
+
+ def add_target_frame_visualization(self) -> None:
+ self.target_frame_handle = self.server.scene.add_frame(
+ "/target_goal", axes_length=0.1, axes_radius=0.003
+ )
+
+ def add_rgb_image_placeholder(self, height: int = 480, width: int = 640) -> None:
+ if self.rgb_image_handle is None:
+ dummy_image = np.zeros((height, width, 3), dtype=np.uint8)
+ self.rgb_image_handle = self.server.gui.add_image(
+ dummy_image, label="RGB Camera", format="jpeg", jpeg_quality=85
+ )
+
+ def update_rgb_image(self, rgb_image: np.ndarray | None) -> None:
+ if rgb_image is None:
+ return
+ if self.rgb_image_handle is None:
+ self.add_rgb_image_placeholder(height=rgb_image.shape[0], width=rgb_image.shape[1])
+ self.rgb_image_handle.image = rgb_image
+
+ def update_robot_pose(self, joint_config: np.ndarray) -> None:
+ self.urdf_vis.update_cfg(joint_config)
+
+ def update_ghost_robot_pose(self, joint_config: np.ndarray) -> None:
+ if self.ghost_robot_urdf:
+ self.ghost_robot_urdf.update_cfg(joint_config)
+
+ def update_ghost_robot_visibility(self, flag: bool) -> None:
+ if self.ghost_robot_urdf:
+ self.ghost_robot_urdf.show_visual = flag
+
+ def get_ghost_robot_visibility(self) -> bool:
+ if self.ghost_robot_urdf:
+ return self.ghost_robot_urdf.show_visual
+ return False
+
+ def set_ghost_robot_color(self, color: tuple[float, float, float, float]) -> None:
+ if self.ghost_robot_urdf:
+ self.ghost_robot_urdf.mesh_color_override = color
+
+ def update_controller_visualization(self, transform: np.ndarray | None) -> None:
+ if self.controller_handle is None or transform is None:
+ return
+ pos = transform[:3, 3]
+ rot = Rotation.from_matrix(transform[:3, :3]).as_quat()
+ self.controller_handle.position = tuple(pos)
+ self.controller_handle.wxyz = (rot[3], rot[0], rot[1], rot[2])
+
+ def update_target_visualization(self, transform: np.ndarray | None) -> None:
+ if self.target_frame_handle is None or transform is None:
+ return
+ pos = transform[:3, 3]
+ rot = Rotation.from_matrix(transform[:3, :3]).as_quat()
+ self.target_frame_handle.position = tuple(pos)
+ self.target_frame_handle.wxyz = (rot[3], rot[0], rot[1], rot[2])
+
+ def stop(self) -> None:
+ self.server.stop()
\ No newline at end of file
diff --git a/examples/common/visualizer_gui.py b/examples/common/visualizer_gui.py
new file mode 100644
index 0000000..7852e99
--- /dev/null
+++ b/examples/common/visualizer_gui.py
@@ -0,0 +1,225 @@
+from typing import Any, Callable
+import numpy as np
+import viser
+
+class RobotVisualizerGUI:
+ """Handles all the 2D UI elements: buttons, sliders, and text readouts."""
+
+ def __init__(self, server: viser.ViserServer) -> None:
+ self.server = server
+ self._ema_timing = 0.001
+
+ # State Handles
+ self._timing_handle = None
+ self._joint_angles_handle = None
+ self._robot_status_handle = None
+ self._teleop_status_handle = None
+ self._controller_status_handle = None
+ self._gripper_status_handle = None
+ self._policy_status_handle = None
+
+ # Input Handles
+ self._position_weight_handle = None
+ self._orientation_weight_handle = None
+ self._frame_task_gain_handle = None
+ self._lm_damping_handle = None
+ self._damping_weight_handle = None
+ self._solver_damping_value_handle = None
+ self._posture_cost_handles = []
+
+ self._controller_min_cutoff_handle = None
+ self._controller_beta_handle = None
+ self._controller_d_cutoff_handle = None
+ self._translation_scale_handle = None
+ self._rotation_scale_handle = None
+
+ self._prediction_ratio_handle = None
+ self._policy_execution_rate_handle = None
+ self._robot_rate_handle = None
+ self._execution_mode_dropdown = None
+
+ self._grip_value_handle = None
+ self._trigger_value_handle = None
+
+ # Button Handles
+ self._enable_robot_handle = None
+ self._disable_robot_handle = None
+ self._emergency_stop_handle = None
+ self._go_home_button = None
+ self._toggle_robot_enabled_status_button = None
+ self._run_policy_button = None
+ self._start_policy_execution_button = None
+ self._play_policy_button = None
+
+ self._slow_translation_scale_handle = None
+ self._slow_rotation_scale_handle = None
+ self._wrist_step_handle = None
+ self._save_config_button = None
+
+ def add_advanced_tuning_controls(self, initial_slow_t: float, initial_slow_r: float, initial_wrist_step: float) -> None:
+ """Adds advanced sliders for precision movement and button toggles."""
+ self._slow_translation_scale_handle = self.server.gui.add_number("Slow Translation Scale", initial_slow_t, min=0.1, max=5.0, step=0.01)
+ self._slow_rotation_scale_handle = self.server.gui.add_number("Slow Rotation Scale", initial_slow_r, min=0.1, max=5.0, step=0.01)
+ self._wrist_step_handle = self.server.gui.add_number("Wrist Nudge (Degrees)", initial_wrist_step, min=1.0, max=45.0, step=1.0)
+ self._save_config_button = self.server.gui.add_button("๐พ Save Config to YAML")
+
+ def get_slow_translation_scale(self) -> float:
+ return self._slow_translation_scale_handle.value
+
+ def get_slow_rotation_scale(self) -> float:
+ return self._slow_rotation_scale_handle.value
+
+ def get_wrist_step_degrees(self) -> float:
+ return self._wrist_step_handle.value
+
+ def set_save_config_callback(self, cb: Callable[[], Any]) -> None:
+ if self._save_config_button:
+ self._save_config_button.on_click(lambda _: cb())
+
+ def add_basic_controls(self) -> None:
+ self._timing_handle = self.server.gui.add_number("IK Solve Time (ms)", 0.001, disabled=True)
+ self._joint_angles_handle = self.server.gui.add_text("Joint Angles", "Waiting...")
+
+ def add_robot_status_controls(self) -> None:
+ self._robot_status_handle = self.server.gui.add_text("Robot Status", "Initializing...")
+
+ def add_teleop_controls(self) -> None:
+ self._grip_value_handle = self.server.gui.add_number("Grip Value", 0.0, disabled=True)
+ self._trigger_value_handle = self.server.gui.add_number("Trigger", 0.0, disabled=True)
+ self._teleop_status_handle = self.server.gui.add_text("Teleop Status", "Inactive")
+ self._controller_status_handle = self.server.gui.add_text("Controller Status", "Waiting...")
+
+ def add_gripper_status_controls(self) -> None:
+ self._gripper_status_handle = self.server.gui.add_text("Gripper Status", "Open (0%)")
+
+ def add_homing_controls(self) -> None:
+ self._go_home_button = self.server.gui.add_button("Go Home")
+
+ def add_toggle_robot_enabled_status_button(self) -> None:
+ self._toggle_robot_enabled_status_button = self.server.gui.add_button("Enable Robot")
+
+ def add_controller_filter_controls(self, initial_min_cutoff: float, initial_beta: float, initial_d_cutoff: float) -> None:
+ self._controller_min_cutoff_handle = self.server.gui.add_number("Controller Min Cutoff", initial_min_cutoff, min=0.01, max=10.0, step=0.01)
+ self._controller_beta_handle = self.server.gui.add_number("Controller Beta", initial_beta, min=0.0, max=10.0, step=0.01)
+ self._controller_d_cutoff_handle = self.server.gui.add_number("Controller D Cutoff", initial_d_cutoff, min=0.01, max=10.0, step=0.01)
+
+ def add_scaling_controls(self, initial_translation_scale: float, initial_rotation_scale: float) -> None:
+ self._translation_scale_handle = self.server.gui.add_number("Translation Scale", initial_translation_scale, min=0.1, max=10.0, step=0.001)
+ self._rotation_scale_handle = self.server.gui.add_number("Rotation Scale", initial_rotation_scale, min=0.1, max=10.0, step=0.001)
+
+ def add_pink_parameter_controls(self, position_cost: float, orientation_cost: float, frame_task_gain: float, lm_damping: float, damping_cost: float, solver_damping_value: float, posture_cost_vector: list[float]) -> None:
+ self._position_weight_handle = self.server.gui.add_number("Position Weight", position_cost, min=0.0, max=10.0, step=0.1)
+ self._orientation_weight_handle = self.server.gui.add_number("Orientation Weight", orientation_cost, min=0.0, max=1.0, step=0.01)
+ self._frame_task_gain_handle = self.server.gui.add_number("Frame Task Gain", frame_task_gain, min=0.0, max=10.0, step=0.1)
+ self._lm_damping_handle = self.server.gui.add_number("LM Damping", lm_damping, min=0.0, max=5.0, step=0.01)
+ self._damping_weight_handle = self.server.gui.add_number("Damping Weight", damping_cost, min=0.0, max=1.0, step=0.01)
+ self._solver_damping_value_handle = self.server.gui.add_number("Solver Damping Value", solver_damping_value, min=0.0, max=1.0, step=0.0001)
+
+ for i, cost in enumerate(posture_cost_vector):
+ self._posture_cost_handles.append(self.server.gui.add_number(f"Posture Cost J{i+1}", cost, min=0.0, max=1.0, step=0.01))
+
+ def add_policy_controls(self, initial_prediction_ratio: float = 0.8, initial_policy_rate: float = 200.0, initial_robot_rate: float = 200.0, initial_execution_mode: str = "targeting_time") -> None:
+ self._policy_status_handle = self.server.gui.add_text("Policy Status", "Ready")
+ self._prediction_ratio_handle = self.server.gui.add_number("Prediction Ratio", initial_prediction_ratio, min=0.0, max=1.0, step=0.01)
+ self._policy_execution_rate_handle = self.server.gui.add_number("Policy Rate", initial_policy_rate, min=1.0, max=200.0, step=1.0)
+ self._robot_rate_handle = self.server.gui.add_number("Robot Rate", initial_robot_rate, min=1.0, max=200.0, step=1.0)
+ self._execution_mode_dropdown = self.server.gui.add_dropdown("Execution Mode", options=["targeting_time", "targeting_pose"], initial_value=initial_execution_mode)
+
+ def add_policy_buttons(self) -> None:
+ self._run_policy_button = self.server.gui.add_button("Run Policy (Preview)")
+ self._start_policy_execution_button = self.server.gui.add_button("Execute Policy (Run Preview)")
+ self._play_policy_button = self.server.gui.add_button("Continuous Receding Horizon")
+
+ # --- UI Update Methods ---
+ def update_timing(self, solve_time_ms: float) -> None:
+ if self._timing_handle:
+ self._ema_timing = 0.99 * self._ema_timing + 0.01 * solve_time_ms
+ self._timing_handle.value = self._ema_timing
+
+ def update_robot_status(self, status: str) -> None:
+ if self._robot_status_handle: self._robot_status_handle.value = status
+
+ def update_teleop_status(self, active: bool) -> None:
+ if self._teleop_status_handle: self._teleop_status_handle.value = f"Teleop Status: {'Active' if active else 'Inactive'}"
+
+ def update_controller_status_display(self, position: np.ndarray | None, connected: bool = True) -> None:
+ if not self._controller_status_handle: return
+ if connected and position is not None:
+ self._controller_status_handle.value = f"Controller Status:\n Position: [{position[0]:.3f}, {position[1]:.3f}, {position[2]:.3f}]\n Connected: โ"
+ else:
+ self._controller_status_handle.value = "Controller Status:\n Connected: โ"
+
+ def update_gripper_status(self, trigger_value: float, robot_enabled: bool = True) -> None:
+ if not self._gripper_status_handle: return
+ state = "Closed" if trigger_value > 0.9 else "Closing" if trigger_value > 0.1 else "Open"
+ status = f"Gripper: {state} ({trigger_value * 100.0:.0f}% closed)"
+ self._gripper_status_handle.value = status + ("" if robot_enabled else " [Disabled]")
+
+ def update_policy_status(self, status: str) -> None:
+ if self._policy_status_handle: self._policy_status_handle.value = status
+
+ def update_toggle_robot_enabled_status(self, enabled: bool) -> None:
+ if self._toggle_robot_enabled_status_button:
+ self._toggle_robot_enabled_status_button.label = "Disable Robot" if enabled else "Enable Robot"
+
+ def update_play_policy_button_status(self, active: bool) -> None:
+ if self._play_policy_button:
+ self._play_policy_button.label = "Stop Continuous Horizon" if active else "Continuous Receding Horizon"
+
+ def update_joint_angles_display(self, joint_config: np.ndarray, show_gripper: bool = False) -> None:
+ if not self._joint_angles_handle: return
+ lines = ["Joint Angles (rad):"]
+ num_joints = len(joint_config) if show_gripper else 6
+ for i in range(num_joints):
+ lbl = f"Joint {i+1} ({'Robot' if i < 6 else 'Gripper'})" if show_gripper else f"J{i+1}"
+ lines.append(f" {lbl}: {joint_config[i]:.3f} rad ({np.degrees(joint_config[i]):.1f}ยฐ)")
+ self._joint_angles_handle.value = "\n".join(lines)
+
+ def set_grip_value(self, value: float) -> None:
+ if self._grip_value_handle: self._grip_value_handle.value = value
+
+ def set_trigger_value(self, value: float) -> None:
+ if self._trigger_value_handle: self._trigger_value_handle.value = value
+
+ # --- Getters ---
+ def get_controller_filter_params(self) -> tuple[float, float, float]:
+ return (self._controller_min_cutoff_handle.value, self._controller_beta_handle.value, self._controller_d_cutoff_handle.value)
+
+ def get_translation_scale(self) -> float: return self._translation_scale_handle.value
+ def get_rotation_scale(self) -> float: return self._rotation_scale_handle.value
+ def get_prediction_ratio(self) -> float: return self._prediction_ratio_handle.value
+ def get_policy_execution_rate(self) -> float: return self._policy_execution_rate_handle.value
+ def get_robot_rate(self) -> float: return self._robot_rate_handle.value
+ def get_execution_mode(self) -> str: return self._execution_mode_dropdown.value
+
+ def get_pink_parameters(self) -> dict:
+ return {
+ "position_cost": self._position_weight_handle.value,
+ "orientation_cost": self._orientation_weight_handle.value,
+ "frame_task_gain": self._frame_task_gain_handle.value,
+ "lm_damping": self._lm_damping_handle.value,
+ "damping_cost": self._damping_weight_handle.value,
+ "solver_damping_value": self._solver_damping_value_handle.value,
+ "posture_cost_vector": np.array([h.value for h in self._posture_cost_handles]),
+ }
+
+ # --- Button Setters/Callbacks ---
+ def set_toggle_robot_enabled_status_callback(self, cb: Callable[[], Any]) -> None:
+ if self._toggle_robot_enabled_status_button: self._toggle_robot_enabled_status_button.on_click(lambda _: cb())
+ def set_go_home_callback(self, cb: Callable[[], Any]) -> None:
+ if self._go_home_button: self._go_home_button.on_click(lambda _: cb())
+ def set_run_policy_callback(self, cb: Callable[[], Any]) -> None:
+ if self._run_policy_button: self._run_policy_button.on_click(lambda _: cb())
+ def set_start_policy_execution_callback(self, cb: Callable[[], Any]) -> None:
+ if self._start_policy_execution_button: self._start_policy_execution_button.on_click(lambda _: cb())
+ def set_play_policy_callback(self, cb: Callable[[], Any]) -> None:
+ if self._play_policy_button: self._play_policy_button.on_click(lambda _: cb())
+ def set_execution_mode_callback(self, cb: Callable[[], Any]) -> None:
+ if self._execution_mode_dropdown: self._execution_mode_dropdown.on_update(lambda _: cb())
+
+ def set_run_policy_button_disabled(self, disabled: bool) -> None:
+ if self._run_policy_button: self._run_policy_button.disabled = disabled
+ def set_start_policy_execution_button_disabled(self, disabled: bool) -> None:
+ if self._start_policy_execution_button: self._start_policy_execution_button.disabled = disabled
+ def set_play_policy_button_disabled(self, disabled: bool) -> None:
+ if self._play_policy_button: self._play_policy_button.disabled = disabled
\ No newline at end of file
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/halid_notes.txt b/halid_notes.txt
new file mode 100644
index 0000000..e69de29
diff --git a/pink_ik_solver.py b/pink_ik_solver.py
index f4bdcfc..f424fe8 100644
--- a/pink_ik_solver.py
+++ b/pink_ik_solver.py
@@ -179,6 +179,22 @@ def _setup_tasks(
print("โ
Tasks configured!")
+ def _rebuild_end_effector_task(self) -> None:
+ """Recreate the FrameTask using current frame-task parameters.
+
+ Some Pink versions expose FrameTask parameters as read-only attributes.
+ Rebuilding the task is the safest way to apply runtime parameter updates.
+ """
+ assert self.configuration is not None, "Configuration must be initialized"
+ self.ee_task = FrameTask(
+ self.end_effector_frame,
+ position_cost=self.position_cost,
+ orientation_cost=self.orientation_cost,
+ lm_damping=self.lm_damping,
+ gain=self.frame_task_gain,
+ )
+ self.ee_task.set_target_from_configuration(self.configuration)
+
def update_task_parameters(
self,
position_cost: float | None = None,
@@ -204,25 +220,26 @@ def update_task_parameters(
"""
assert self.ee_task is not None, "End effector task must be initialized"
assert self.damping_task is not None, "Damping task must be initialized"
+ should_rebuild_ee_task = False
if position_cost is not None:
self.position_cost = position_cost
- self.ee_task.position_cost = position_cost
+ should_rebuild_ee_task = True
if orientation_cost is not None:
self.orientation_cost = orientation_cost
- self.ee_task.orientation_cost = orientation_cost
+ should_rebuild_ee_task = True
if frame_task_gain is not None:
self.frame_task_gain = frame_task_gain
- self.ee_task.gain = frame_task_gain
+ should_rebuild_ee_task = True
if lm_damping is not None:
self.lm_damping = lm_damping
- self.ee_task.lm_damping = lm_damping
+ should_rebuild_ee_task = True
if damping_cost is not None:
self.damping_cost = damping_cost
- self.damping_task.cost = damping_cost
+ self.damping_task = DampingTask(cost=self.damping_cost)
if solver_damping_value is not None:
self.solver_damping_value = solver_damping_value
@@ -240,6 +257,9 @@ def update_task_parameters(
assert self.posture_task is not None, "Posture task must be initialized"
self.posture_task.cost = self.posture_cost_vector
+ if should_rebuild_ee_task:
+ self._rebuild_end_effector_task()
+
def set_target_pose(self, position: np.ndarray, orientation: np.ndarray) -> None:
"""Set target pose from position and orientation.
diff --git a/piper_controller.py b/piper_controller.py
index 980725c..3e37f93 100644
--- a/piper_controller.py
+++ b/piper_controller.py
@@ -29,8 +29,9 @@ def __init__(
can_interface: str = "can0",
robot_rate: float = 100.0,
control_mode: "PiperController.ControlMode" = ControlMode.JOINT_SPACE,
- neutral_joint_angles: np.ndarray | None = None,
- neutral_end_effector_pose: np.ndarray | None = None,
+ enable_joint_angle_limits: bool = True,
+ neutral_joint_angles: np.ndarray | list[float] | None = None,
+ neutral_end_effector_pose: np.ndarray | list[float] | None = None,
debug_mode: bool = False,
) -> None:
"""Initialize the robot controller.
@@ -39,13 +40,15 @@ def __init__(
can_interface: CAN interface for robot communication (default: 'can0')
robot_rate: Robot control loop rate in Hz (default: 100.0)
control_mode: Initial control mode (END_EFFECTOR or JOINT_SPACE)
+ enable_joint_angle_limits: Enable SDK + software joint angle limits (default: True)
neutral_joint_angles: Neutral joint angles [j1, j2, j3, j4, j5, j6] in degrees (default: None)
- neutral_end_effector_pose: Neutral end effector pose as 4x4 transformation matrix (default: None)
+ neutral_end_effector_pose: Neutral end effector 6D pose [x, y, z, rx, ry, rz] in mm/degrees (default: None)
debug_mode: Enable debug logging (default: False)
"""
self.can_interface = can_interface
self.robot_rate = robot_rate
self.debug_mode = debug_mode
+ self.enable_joint_angle_limits = enable_joint_angle_limits
# Thread synchronization
self.position_lock = threading.Lock()
@@ -65,11 +68,12 @@ def __init__(
# HOME positions in end effector space and joint space
if neutral_end_effector_pose is not None:
- if neutral_end_effector_pose.shape == (4, 4):
- self.HOME_POSE = neutral_end_effector_pose.copy().astype(np.float64)
+ neutral_pose_6d = np.array(neutral_end_effector_pose, dtype=np.float64)
+ if neutral_pose_6d.shape == (6,):
+ self.HOME_POSE = self._pose_6d_to_4x4(neutral_pose_6d)
else:
raise ValueError(
- "neutral_end_effector_pose must be a 4x4 transformation matrix"
+ "neutral_end_effector_pose must be a 6D pose [x, y, z, rx, ry, rz]"
)
else:
# Convert default 6D pose to 4x4 matrix
@@ -86,8 +90,8 @@ def __init__(
)
# Gripper range in degrees (for internal SDK communication)
- self.GRIPPER_DEGREES_MIN = -5.000
- self.GRIPPER_DEGREES_MAX = 95.00
+ self.GRIPPER_DEGREES_MIN = -10.00
+ self.GRIPPER_DEGREES_MAX = 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
@@ -169,7 +173,9 @@ def _initialize_robot(self) -> None:
"""Initialize robot connection and enable it."""
print(f"Initializing robot on {self.can_interface}...")
self.piper = C_PiperInterface_V2(
- self.can_interface, start_sdk_joint_limit=True, start_sdk_gripper_limit=True
+ self.can_interface,
+ start_sdk_joint_limit=self.enable_joint_angle_limits,
+ start_sdk_gripper_limit=False,
)
self.piper.ConnectPort()
@@ -217,18 +223,34 @@ def get_control_mode(self) -> "PiperController.ControlMode":
with self.state_lock:
return self._control_mode
- def set_control_mode(self, mode: "PiperController.ControlMode") -> None:
- """Set the control mode.
+ # def set_control_mode(self, mode: "PiperController.ControlMode") -> None:
+ # """Set the control mode.
- Args:
- mode: Control mode (PiperController.ControlMode.END_EFFECTOR or PiperController.ControlMode.JOINT_SPACE)
- """
+ # Args:
+ # mode: Control mode (PiperController.ControlMode.END_EFFECTOR or PiperController.ControlMode.JOINT_SPACE)
+ # """
+ # with self.state_lock:
+ # old_mode = self._control_mode
+ # self._control_mode = mode
+ # if self.debug_mode:
+ # print(f"Control mode changed: {old_mode.value} -> {mode.value}")
+
+
+ def set_control_mode(self, mode: "PiperController.ControlMode") -> None:
with self.state_lock:
old_mode = self._control_mode
self._control_mode = mode
+
+ # Send the mode configuration ONCE here, not in the 100Hz loop
+ if hasattr(self, "piper") and self.piper.get_connect_status():
+ if mode == PiperController.ControlMode.JOINT_SPACE:
+ self.piper.MotionCtrl_2(0x01, 0x01, 100, 0x00)
+ elif mode == PiperController.ControlMode.END_EFFECTOR:
+ self.piper.MotionCtrl_2(0x01, 0x00, 100, 0x00)
+
if self.debug_mode:
print(f"Control mode changed: {old_mode.value} -> {mode.value}")
-
+
@staticmethod
def _pose_6d_to_4x4(pose_6d: np.ndarray) -> np.ndarray:
"""Convert 6D pose [x, y, z, rx, ry, rz] to 4x4 transformation matrix.
@@ -397,12 +419,15 @@ def set_target_joint_angles(self, joint_angles: np.ndarray) -> None:
with self.position_lock:
angles = np.array(joint_angles, dtype=np.float64)
- # Clamp joint angles to limits using numpy
- clamped_angles = np.clip(
- angles, self.JOINT_LIMITS[:, 0], self.JOINT_LIMITS[:, 1]
- )
+ if self.enable_joint_angle_limits:
+ # Clamp joint angles to limits using numpy
+ target_angles = np.clip(
+ angles, self.JOINT_LIMITS[:, 0], self.JOINT_LIMITS[:, 1]
+ )
+ else:
+ target_angles = angles
- self._target_joint_angles = clamped_angles
+ self._target_joint_angles = target_angles
if self.debug_mode:
print(f"Target joint angles set: {self._target_joint_angles}")
@@ -417,10 +442,13 @@ def update_target_joint_angles(self, joint_deltas: np.ndarray) -> None:
deltas = np.array(joint_deltas, dtype=np.float64)
new_joint_angles = self._target_joint_angles + deltas
- # Clamp joint angles to limits using numpy
- self._target_joint_angles = np.clip(
- new_joint_angles, self.JOINT_LIMITS[:, 0], self.JOINT_LIMITS[:, 1]
- )
+ if self.enable_joint_angle_limits:
+ # Clamp joint angles to limits using numpy
+ self._target_joint_angles = np.clip(
+ new_joint_angles, self.JOINT_LIMITS[:, 0], self.JOINT_LIMITS[:, 1]
+ )
+ else:
+ self._target_joint_angles = new_joint_angles
if self.debug_mode:
print(f"Joint angles updated: {self._target_joint_angles}")
@@ -465,11 +493,23 @@ def control_loop(self) -> None:
Only sends commands when robot is in ENABLED state.
"""
loop_period = 1.0 / self.robot_rate
+ # Re-enable heartbeat every 1 second to recover from robot self-disable
+ reenable_interval = 1.0
+ last_reenable_time = time.time()
while self.running.is_set():
try:
# Only send commands if robot is enabled
if self.is_robot_enabled():
+ now = time.time()
+ # Periodically re-send EnablePiper to keep hardware enabled
+ # (robot firmware can self-disable on faults/limits)
+ if now - last_reenable_time >= reenable_interval:
+ if not self.piper.EnablePiper():
+ if self.debug_mode:
+ print("Control loop: EnablePiper re-enable returned False")
+ last_reenable_time = now
+
# Get current control mode
current_mode = self.get_control_mode()
@@ -479,7 +519,7 @@ def control_loop(self) -> None:
self._send_end_effector_command(self._target_pose)
elif current_mode == PiperController.ControlMode.JOINT_SPACE:
# Send joint angles command
- self._send_joint_command(self._target_joint_angles)
+ self._send_joint_command(self._target_joint_angles.copy())
# Always send gripper command regardless of control mode
self._send_gripper_command(self._gripper_open_value_degrees)
@@ -708,6 +748,29 @@ def get_current_joint_angles(self) -> np.ndarray | None:
print(f"Failed to get current joint angles: {e}")
return None
+ def get_current_joint_currents(self) -> np.ndarray | None:
+ """Get the current measured motor currents from the robot, if available.
+
+ Returns:
+ numpy array or None: [m1, m2, m3, m4, m5, m6] in amps, or None if unavailable
+ """
+ if hasattr(self, "piper") and self.piper is not None:
+ try:
+ high_spd_msg = self.piper.GetArmHighSpdInfoMsgs()
+ if high_spd_msg:
+ # Current is reported in 0.001A units per motor
+ motors = []
+ for i in range(1, 7):
+ motor = getattr(high_spd_msg, f"motor_{i}", None)
+ if motor is None:
+ return None
+ motors.append(motor.current / 1000.0)
+ return np.array(motors, dtype=np.float64)
+ except Exception as e:
+ if self.debug_mode:
+ print(f"Failed to get current motor currents: {e}")
+ return None
+
def get_current_gripper_open_value(self) -> float | None:
"""Get the current measured gripper open value from the robot, if available.
@@ -739,11 +802,13 @@ def get_robot_status(self) -> dict[str, Any]:
status = {
"enabled": self.is_robot_enabled(),
"control_mode": self.get_control_mode(),
+ "joint_angle_limits_enabled": self.enable_joint_angle_limits,
"target_pose": self.get_target_pose(),
"target_joint_angles": self.get_target_joint_angles(),
"gripper_open_value": self.get_gripper_open_value(),
"current_end_pose": self.get_current_end_effector_pose(),
"current_joint_angles": self.get_current_joint_angles(),
+ "current_joint_currents": self.get_current_joint_currents(),
"current_gripper_open_value": self.get_current_gripper_open_value(),
}
return status
@@ -753,11 +818,13 @@ def get_robot_status(self) -> dict[str, Any]:
return {
"enabled": None,
"control_mode": None,
+ "joint_angle_limits_enabled": None,
"target_pose": None,
"target_joint_angles": None,
"gripper_open_value": None,
"current_end_pose": None,
"current_joint_angles": None,
+ "current_joint_currents": None,
"current_gripper_open_value": None,
}
diff --git a/scripts/piper/can_activate.sh b/scripts/piper/can_activate.sh
index 1a957d1..bf254fe 100644
--- a/scripts/piper/can_activate.sh
+++ b/scripts/piper/can_activate.sh
@@ -121,16 +121,18 @@ else
# Set the interface bitrate and activate it.
sudo ip link set "$INTERFACE_NAME" down
- sudo ip link set "$INTERFACE_NAME" type can bitrate $DEFAULT_BITRATE
+ sudo ip link set "$INTERFACE_NAME" type can bitrate $DEFAULT_BITRATE restart-ms 100
sudo ip link set "$INTERFACE_NAME" up
+ sudo ip link set "$INTERFACE_NAME" txqueuelen 1000
echo "Interface $INTERFACE_NAME has been reset to bitrate $DEFAULT_BITRATE and activated."
-
+
# Rename the interface to the default name.
if [ "$INTERFACE_NAME" != "$DEFAULT_CAN_NAME" ]; then
echo "Rename interface $INTERFACE_NAME to $DEFAULT_CAN_NAME."
sudo ip link set "$INTERFACE_NAME" down
sudo ip link set "$INTERFACE_NAME" name "$DEFAULT_CAN_NAME"
sudo ip link set "$DEFAULT_CAN_NAME" up
+ sudo ip link set "$DEFAULT_CAN_NAME" txqueuelen 1000
echo "The interface has been renamed to $DEFAULT_CAN_NAME and reactivated."
fi
fi
diff --git a/scripts/piper/piper_gui_control.py b/scripts/piper/piper_gui_control.py
index 2f4d1fc..b1ab17e 100644
--- a/scripts/piper/piper_gui_control.py
+++ b/scripts/piper/piper_gui_control.py
@@ -325,6 +325,22 @@ def _create_gui(self) -> None:
)
self.home_button.grid(row=0, column=1, padx=10)
+ # Copy current joint + gripper values to clipboard
+ self.copy_pose_button = ttk.Button(
+ button_frame,
+ text="Copy Joint+Gripper",
+ command=self._on_copy_joint_gripper_pressed,
+ )
+ self.copy_pose_button.grid(row=0, column=2, padx=10)
+
+ # Copy current end-effector pose to clipboard
+ self.copy_ee_position_button = ttk.Button(
+ button_frame,
+ text="Copy EE Pose",
+ command=self._on_copy_ee_position_pressed,
+ )
+ self.copy_ee_position_button.grid(row=0, column=3, padx=10)
+
# Configure grid weights
self.root.columnconfigure(0, weight=1)
self.root.rowconfigure(0, weight=1)
@@ -718,6 +734,83 @@ def _on_stop_resume_pressed(self) -> None:
2000, lambda: self.status_label.config(text="Ready", foreground="green")
)
+ def _on_copy_joint_gripper_pressed(self) -> None:
+ """Copy current joint positions and gripper value to clipboard."""
+ status = self.robot.get_robot_status()
+
+ joint_angles = status["current_joint_angles"]
+ if joint_angles is None:
+ joint_angles = status["target_joint_angles"]
+
+ gripper_value = status["current_gripper_open_value"]
+ if gripper_value is None:
+ gripper_value = status["gripper_open_value"]
+
+ if joint_angles is None or gripper_value is None:
+ self.status_label.config(
+ text="No joint/gripper data available to copy", foreground="red"
+ )
+ self.root.after(
+ 2000, lambda: self.status_label.config(text="Ready", foreground="green")
+ )
+ return
+
+ joint_text = ", ".join(f"{float(angle):.3f}" for angle in joint_angles)
+ clipboard_text = (
+ f"joint_positions: {joint_text}\n"
+ f"gripper_value: {float(gripper_value):.3f}"
+ )
+
+ self.root.clipboard_clear()
+ self.root.clipboard_append(clipboard_text)
+ self.root.update()
+
+ self.status_label.config(
+ text="Copied joint positions and gripper to clipboard", foreground="blue"
+ )
+ self.root.after(
+ 2000, lambda: self.status_label.config(text="Ready", foreground="green")
+ )
+
+ def _on_copy_ee_position_pressed(self) -> None:
+ """Copy current end-effector pose (XYZ + RPY) to clipboard."""
+ status = self.robot.get_robot_status()
+
+ pose = status["current_end_pose"]
+ if pose is None:
+ pose = status["target_pose"]
+
+ if pose is None:
+ self.status_label.config(
+ text="No end-effector pose available to copy", foreground="red"
+ )
+ self.root.after(
+ 2000, lambda: self.status_label.config(text="Ready", foreground="green")
+ )
+ return
+
+ position = pose[:3, 3]
+ rotation = Rotation.from_matrix(pose[:3, :3]).as_euler("xyz", degrees=True)
+ clipboard_text = (
+ f"ee_position: x={float(position[0]):.3f}, "
+ f"y={float(position[1]):.3f}, "
+ f"z={float(position[2]):.3f}\n"
+ f"ee_rotation_xyz_deg: rx={float(rotation[0]):.3f}, "
+ f"ry={float(rotation[1]):.3f}, "
+ f"rz={float(rotation[2]):.3f}"
+ )
+
+ self.root.clipboard_clear()
+ self.root.clipboard_append(clipboard_text)
+ self.root.update()
+
+ self.status_label.config(
+ text="Copied end-effector pose to clipboard", foreground="blue"
+ )
+ self.root.after(
+ 2000, lambda: self.status_label.config(text="Ready", foreground="green")
+ )
+
def command_handler(self) -> None:
"""Handle commands from GUI in separate thread."""
dt = 1.0 / self.control_update_rate