diff --git a/CMakeLists.txt b/CMakeLists.txt index 998e78b39c..18bdda30b9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -184,6 +184,7 @@ if(BUILD_PLUGINS) add_subdirectory(src/plugins/controller_se3_tracker) add_subdirectory(src/plugins/controller_synthetic_hands) + add_subdirectory(src/plugins/gamepad) add_subdirectory(src/plugins/generic_3axis_pedal) add_subdirectory(src/plugins/so101_leader) add_subdirectory(src/plugins/rebot_devarm_leader) diff --git a/examples/teleop/python/gamepad_printer_example.py b/examples/teleop/python/gamepad_printer_example.py new file mode 100644 index 0000000000..cfd4c1ada8 --- /dev/null +++ b/examples/teleop/python/gamepad_printer_example.py @@ -0,0 +1,127 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Gamepad Printer Example. + +Prints every currently-held button and the full axis array each frame, via +GamepadSource's "gamepad_buttons" and "gamepad_axes" outputs. Carries no semantic +mapping (stick, trigger, toggle) -- that belongs in a retargeter (e.g. +GamepadToSe3RelRetargeter) consuming this source's output. The gamepad plugin +self-discovers its device and is auto-launched by TeleopSession -- no external +process to start manually. +""" + +import sys +import time +from pathlib import Path + +from isaacteleop.cloudxr import CloudXRLauncher +from isaacteleop.retargeting_engine.deviceio_source_nodes import GamepadSource +from isaacteleop.teleop_session_manager import ( + TeleopSession, + TeleopSessionConfig, + PluginConfig, +) + + +PLUGIN_ROOT_DIR = Path(__file__).resolve().parent.parent.parent.parent / "plugins" +PLUGIN_NAME = "gamepad" +PLUGIN_ROOT_ID = "gamepad" + + +def main(): + import argparse + + parser = argparse.ArgumentParser(description=__doc__) + CloudXRLauncher.add_launcher_arguments(parser) + args = parser.parse_args() + + print("\n" + "=" * 80) + print(" Gamepad Printer Example") + print("=" * 80) + print("Press any button or move a stick on the connected gamepad.") + print("=" * 80 + "\n") + + # ================================================================== + # Setup: Create gamepad source + # ================================================================== + gamepad_source = GamepadSource(name="gamepad") + + # ================================================================== + # Configure Plugins + # ================================================================== + + plugins = [] + if PLUGIN_ROOT_DIR.exists(): + plugins.append( + PluginConfig( + plugin_name=PLUGIN_NAME, + plugin_root_id=PLUGIN_ROOT_ID, + search_paths=[PLUGIN_ROOT_DIR], + ) + ) + + # ================================================================== + # Create and run TeleopSession + # ================================================================== + + session_config = TeleopSessionConfig( + app_name="GamepadPrinterExample", + trackers=[], + pipeline=gamepad_source, + plugins=plugins, + ) + + with CloudXRLauncher.launch_context(args): + with TeleopSession(session_config) as session: + start_time = time.time() + prev_pressed: set[int] = set() + + while time.time() - start_time < 30.0: + result = session.step() + buttons_group = result["gamepad_buttons"] + axes_group = result["gamepad_axes"] + + elapsed = session.get_elapsed_time() + if buttons_group.is_none: + print( + f"[{elapsed:5.1f}s] (no gamepad data yet)", + end="\r", + flush=True, + ) + time.sleep(0.01) + continue + + bitmap = buttons_group[0] + axes = axes_group[0] + pressed = {code for code in range(len(bitmap)) if bitmap[code]} + axes_str = " ".join(f"{v:+.2f}" for v in axes) + + # Live status line (overwritten each frame). + names = [f"btn{code}" for code in sorted(pressed)] + print( + f"[{elapsed:5.1f}s] Axes: [{axes_str}] Held: {' '.join(names) or '-'}" + + " " * 20, + end="\r", + flush=True, + ) + + # Permanent, scrollable log of every press/release transition -- a + # quick tap can flash by on the status line above before you notice + # it, but every transition is logged here. + for code in sorted(pressed - prev_pressed): + print(f"[{elapsed:5.1f}s] btn{code} down") + for code in sorted(prev_pressed - pressed): + print(f"[{elapsed:5.1f}s] btn{code} up") + prev_pressed = pressed + + time.sleep(0.01) # ~100 FPS + + print("\nTime limit reached.") + + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/core/deviceio_trackers/trackers.toml b/src/core/deviceio_trackers/trackers.toml index 813ec47581..47ed1702c4 100644 --- a/src/core/deviceio_trackers/trackers.toml +++ b/src/core/deviceio_trackers/trackers.toml @@ -28,6 +28,12 @@ traits = "PedalRecordingTraits" max_flatbuffer_size = 256 python_accessor = "get_pedal_data" +[[tracker]] +name = "gamepad" +table = "GamepadOutput" +max_flatbuffer_size = 512 +python_accessor = "get_gamepad_data" + [[tracker]] name = "haptic_command" direction = "push" diff --git a/src/core/schema/fbs/gamepad.fbs b/src/core/schema/fbs/gamepad.fbs new file mode 100644 index 0000000000..d1f381d2cc --- /dev/null +++ b/src/core/schema/fbs/gamepad.fbs @@ -0,0 +1,33 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "timestamp.fbs"; + +namespace core; + +// Raw state of a Linux joystick-API gamepad (e.g. /dev/input/js0): the set of +// currently-held button indices and the current value of every reported axis. +// Carries no semantic mapping -- which axis/button means what (a stick, a trigger, +// a toggle) is entirely up to the consuming retargeter. +// +// All fields are always present whenever this table itself is present. +table GamepadOutput { + // Button indices (Linux joystick JS_EVENT_BUTTON numbers) currently held down, + // in no particular order. + pressed_buttons: [ushort] (id: 0); + + // Current value of every reported axis (Linux joystick JS_EVENT_AXIS numbers), + // normalized to [-1, 1]. Index i is the value of axis i. + axes: [float] (id: 1); + + // Whether the tracker has emitted at least one sample. + is_valid: bool (id: 2); +} + +// MCAP recording wrapper for GamepadOutput. +table GamepadOutputRecord { + data: GamepadOutput (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type GamepadOutputRecord; diff --git a/src/core/schema/python/CMakeLists.txt b/src/core/schema/python/CMakeLists.txt index 5f01ee3110..1753009775 100644 --- a/src/core/schema/python/CMakeLists.txt +++ b/src/core/schema/python/CMakeLists.txt @@ -5,6 +5,7 @@ pybind11_add_module(schema_py oak_bindings.h controller_bindings.h full_body_bindings.h + gamepad_bindings.h hand_bindings.h haptic_command_bindings.h head_bindings.h diff --git a/src/core/schema/python/gamepad_bindings.h b/src/core/schema/python/gamepad_bindings.h new file mode 100644 index 0000000000..b98b1a0620 --- /dev/null +++ b/src/core/schema/python/gamepad_bindings.h @@ -0,0 +1,71 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Python bindings for the Gamepad FlatBuffer schema. +// Types: GamepadOutput (table), exposed as an encoded view. + +#pragma once + +#include "schema_serialized.h" + +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_gamepad(py::module& m) +{ + serialized_class(m, "GamepadOutput", "Encoded raw joystick-API button/axis state.") + .def(py::init( + [](std::vector pressed_buttons, std::vector axes, bool is_valid) + { + GamepadOutputT native; + native.pressed_buttons = std::move(pressed_buttons); + native.axes = std::move(axes); + native.is_valid = is_valid; + return pack(native); + }), + py::arg("pressed_buttons"), py::arg("axes"), py::arg("is_valid"), "Encode a gamepad button/axis snapshot.") + .def_property_readonly("pressed_buttons", vector_field(&GamepadOutput::pressed_buttons)) + .def_property_readonly("axes", vector_field(&GamepadOutput::axes)) + .def_property_readonly("is_valid", field(&GamepadOutput::is_valid)) + .def("__repr__", + [](const Serialized& self) + { + std::string result = "GamepadOutput(pressed_buttons=["; + const auto* buttons = self->pressed_buttons(); + if (buttons != nullptr) + { + for (size_t i = 0; i < buttons->size(); ++i) + { + if (i > 0) + result += ", "; + result += std::to_string((*buttons)[i]); + } + } + result += "], axes=["; + const auto* axes = self->axes(); + if (axes != nullptr) + { + for (size_t i = 0; i < axes->size(); ++i) + { + if (i > 0) + result += ", "; + result += std::to_string((*axes)[i]); + } + } + result += "], is_valid=" + std::to_string(self->is_valid()) + ")"; + return result; + }); + + bind_record(m, "GamepadOutputRecord", "GamepadOutput"); +} + +} // namespace core diff --git a/src/core/schema/python/schema_module.cpp b/src/core/schema/python/schema_module.cpp index 420d2b6965..34e0f3a449 100644 --- a/src/core/schema/python/schema_module.cpp +++ b/src/core/schema/python/schema_module.cpp @@ -8,6 +8,7 @@ // Include binding definitions. #include "controller_bindings.h" #include "full_body_bindings.h" +#include "gamepad_bindings.h" #include "hand_bindings.h" #include "haptic_command_bindings.h" #include "head_bindings.h" @@ -44,6 +45,9 @@ PYBIND11_MODULE(_schema, m) // Bind pedals types (Generic3AxisPedalOutput table). core::bind_pedals(m); + // Bind gamepad types (GamepadOutput table) for raw joystick-API button/axis state. + core::bind_gamepad(m); + // Bind OGLO tactile glove types (OgloGloveSample table). core::bind_oglo_tactile(m); diff --git a/src/plugins/gamepad/CMakeLists.txt b/src/plugins/gamepad/CMakeLists.txt new file mode 100644 index 0000000000..b2e18ac31b --- /dev/null +++ b/src/plugins/gamepad/CMakeLists.txt @@ -0,0 +1,23 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +if(NOT CMAKE_SYSTEM_NAME STREQUAL "Linux") + message(STATUS "Skipping gamepad plugin (Linux only)") + add_custom_target(gamepad_plugin + COMMAND ${CMAKE_COMMAND} -E echo "Skipping gamepad: Linux only") + return() +endif() + +add_executable(gamepad_plugin + main.cpp + gamepad_plugin.cpp +) + +target_link_libraries(gamepad_plugin PRIVATE + pusherio::pusherio + oxr::oxr_core + isaacteleop_schema +) + +install(TARGETS gamepad_plugin RUNTIME DESTINATION plugins/gamepad) +install(FILES plugin.yaml README.md DESTINATION plugins/gamepad) diff --git a/src/plugins/gamepad/README.md b/src/plugins/gamepad/README.md new file mode 100644 index 0000000000..07f1ff16d8 --- /dev/null +++ b/src/plugins/gamepad/README.md @@ -0,0 +1,46 @@ + + +# Gamepad Plugin + +Reads a gamepad from `/dev/input/js*` (Linux joystick API) and pushes `GamepadOutput` via OpenXR. +Use with `GamepadTracker` with the same `collection_id`. + +Reports raw button/axis state only, with no semantic mapping to sticks, triggers, or commands -- +that mapping belongs in a retargeter (e.g. `GamepadToSe3RelRetargeter`) consuming this tracker's +output. + +Self-discovers its device (the first `*-joystick` entry under `/dev/input/by-path/`), so it needs +no arguments to run and can be auto-launched by `PluginManager` via `PluginConfig` -- no manual +process to start. + +## Usage + +Auto-launched (recommended -- matches how `PluginManager` invokes plugins): + +```bash +./gamepad_plugin --plugin-root-id=gamepad +``` + +Manual / standalone, with an explicit device: + +```bash +./gamepad_plugin [device_path] [--plugin-root-id=] +``` + +- **device_path**: Optional. Defaults to the first `*-joystick` entry under `/dev/input/by-path/`. + Identify a specific gamepad with `cat /proc/bus/input/devices` (look for a `Handlers=... jsN` + line under a gamepad entry) or `jstest /dev/input/jsN`. Reading `/dev/input/js*` typically + requires membership in the `input` group. +- **collection_id**: Default `gamepad`. Match this when creating `GamepadTracker`. + +## Button/axis mapping + +Reports every axis value (normalized to `[-1, 1]`) and the set of currently-held button indices, +as reported by the Linux joystick API (`JS_EVENT_AXIS` / `JS_EVENT_BUTTON`, see +`linux/joystick.h`). Axis/button indices and count depend on the connected device's driver (e.g. +`xpad` for Xbox-style controllers) -- no fixed mapping is assumed here. + +Linux only. diff --git a/src/plugins/gamepad/gamepad_plugin.cpp b/src/plugins/gamepad/gamepad_plugin.cpp new file mode 100644 index 0000000000..9edcf8aaa5 --- /dev/null +++ b/src/plugins/gamepad/gamepad_plugin.cpp @@ -0,0 +1,177 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "gamepad_plugin.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace gamepad +{ + +namespace +{ + +constexpr size_t kJsEventSize = sizeof(js_event); +constexpr double kMaxAxisValue = 32767.0; +constexpr size_t kMaxFlatbufferSize = 512; +// Fallback axis count when JSIOCGAXES is unavailable -- covers the common +// left/right-stick + trigger + dpad layout (8 axes) reported by most +// Xbox-style gamepads under the xpad driver. +constexpr uint8_t kDefaultAxisCount = 8; + +double normalize_axis(int16_t raw_value) +{ + return std::max(-1.0, std::min(1.0, static_cast(raw_value) / kMaxAxisValue)); +} + +} // namespace + +GamepadPlugin::GamepadPlugin(const std::string& device_path, const std::string& collection_id) + : device_path_(device_path), + session_(std::make_shared("GamepadPlugin", core::SchemaPusher::get_required_extensions())), + pusher_(session_->get_handles(), + core::SchemaPusherConfig{ .collection_id = collection_id, + .max_flatbuffer_size = kMaxFlatbufferSize, + .tensor_identifier = "gamepad", + .localized_name = "Gamepad", + .app_name = "GamepadPlugin" }) +{ + if (!open_device()) + throw std::runtime_error("GamepadPlugin: Failed to open " + device_path + " (" + strerror(errno) + ")"); +} + +GamepadPlugin::~GamepadPlugin() +{ + if (device_fd_ >= 0) + close_device(); +} + +void GamepadPlugin::update() +{ + if (device_fd_ < 0) + { + open_device(); + if (device_fd_ < 0) + { + push_current_state(); + return; + } + } + + fd_set read_fds; + struct timeval timeout = { 0, 0 }; + + while (true) + { + FD_ZERO(&read_fds); + FD_SET(device_fd_, &read_fds); + timeout = { 0, 0 }; + + int ret = select(device_fd_ + 1, &read_fds, nullptr, nullptr, &timeout); + if (ret < 0) + { + if (errno == EINTR) + return; + close_device(); + push_current_state(); + return; + } + if (ret == 0 || !FD_ISSET(device_fd_, &read_fds)) + { + // If there is no data to read (ret == 0) or the device file descriptor is not set in + // the read set, break out of the loop; this means there's no new event available. + break; + } + + js_event event; + ssize_t n = read(device_fd_, &event, kJsEventSize); + if (n != static_cast(kJsEventSize)) + { + if (n < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) + break; + close_device(); + push_current_state(); + return; + } + + const auto type = static_cast(event.type & ~JS_EVENT_INIT); + if (type == JS_EVENT_AXIS && event.number < axes_.size()) + { + axes_[event.number] = static_cast(normalize_axis(event.value)); + } + else if (type == JS_EVENT_BUTTON) + { + if (event.value != 0) + pressed_buttons_.insert(event.number); + else + pressed_buttons_.erase(event.number); + } + } + + push_current_state(); +} + +bool GamepadPlugin::open_device() +{ + assert(device_fd_ < 0); + + int fd = open(device_path_.c_str(), O_RDONLY | O_NONBLOCK); + if (fd < 0) + return false; + + uint8_t axis_count = kDefaultAxisCount; + ioctl(fd, JSIOCGAXES, &axis_count); + axes_.assign(axis_count, 0.0f); + + device_fd_ = fd; + std::cout << "GamepadPlugin: Opened " << device_path_ << " (" << static_cast(axis_count) << " axes)" + << std::endl; + return true; +} + +void GamepadPlugin::close_device() +{ + assert(device_fd_ >= 0); + + close(device_fd_); + device_fd_ = -1; + // A closed device can no longer report releases -- forget everything it + // last reported as held so a stale button doesn't stick "pressed" forever. + pressed_buttons_.clear(); + // Likewise, a disconnected gamepad can no longer report the stick returning to + // center -- reset axes to neutral so a stale nonzero reading can't keep commanding + // motion after the device is gone. + axes_.assign(axes_.size(), 0.0F); +} + +void GamepadPlugin::push_current_state() +{ + core::GamepadOutputT out; + out.pressed_buttons.assign(pressed_buttons_.begin(), pressed_buttons_.end()); + out.axes = axes_; + out.is_valid = true; + + auto sample_time_ns = core::os_monotonic_now_ns(); + + flatbuffers::FlatBufferBuilder builder(kMaxFlatbufferSize); + auto offset = core::GamepadOutput::Pack(builder, &out); + builder.Finish(offset); + pusher_.push_buffer(builder.GetBufferPointer(), builder.GetSize(), sample_time_ns, sample_time_ns); +} + +} // namespace gamepad +} // namespace plugins diff --git a/src/plugins/gamepad/gamepad_plugin.hpp b/src/plugins/gamepad/gamepad_plugin.hpp new file mode 100644 index 0000000000..ae06825360 --- /dev/null +++ b/src/plugins/gamepad/gamepad_plugin.hpp @@ -0,0 +1,54 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace core +{ +class OpenXRSession; +} + +namespace plugins +{ +namespace gamepad +{ + +/*! + * @brief Reads a Linux joystick-API gamepad device (e.g. /dev/input/js0), tracks + * the set of currently-held button indices and every reported axis value, + * and pushes GamepadOutput via OpenXR SchemaPusher. Carries no semantic + * mapping -- buttons/axes are reported as-is (Linux joystick API indices). + */ +class GamepadPlugin +{ +public: + GamepadPlugin(const std::string& device_path, const std::string& collection_id); + ~GamepadPlugin(); + + void update(); + +private: + bool open_device(); + void close_device(); + void push_current_state(); + + std::string device_path_; + int device_fd_ = -1; + + std::set pressed_buttons_; + std::vector axes_; + + std::shared_ptr session_; + core::SchemaPusher pusher_; +}; + +} // namespace gamepad +} // namespace plugins diff --git a/src/plugins/gamepad/main.cpp b/src/plugins/gamepad/main.cpp new file mode 100644 index 0000000000..9607b77bdd --- /dev/null +++ b/src/plugins/gamepad/main.cpp @@ -0,0 +1,128 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "gamepad_plugin.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::gamepad; + +namespace +{ + +// Returns the first udev-classified joystick device (js API, not evdev), or +// nullopt if none found. Lets the plugin run with zero required arguments +// when launched via PluginManager (which invokes plugins as +// ` --plugin-root-id=`, not positional args). +std::optional discover_gamepad_device_path() +{ + const std::filesystem::path by_path_dir = "/dev/input/by-path"; + std::vector candidates; + std::error_code ec; + if (!std::filesystem::exists(by_path_dir, ec)) + return std::nullopt; + + for (const auto& entry : std::filesystem::directory_iterator(by_path_dir, ec)) + { + const std::string name = entry.path().filename().string(); + // "*-event-joystick" (evdev, /dev/input/eventN) also ends with "-joystick" and + // would otherwise be picked up alongside "*-joystick" (js API, /dev/input/jsN, + // what this plugin's js_event-based reader actually needs) -- exclude it + // explicitly rather than relying on suffix matching alone. + if (name.ends_with("-joystick") && !name.ends_with("-event-joystick")) + candidates.push_back(entry.path().string()); + } + if (candidates.empty()) + return std::nullopt; + + std::sort(candidates.begin(), candidates.end()); + return candidates.front(); +} + +// PluginManager invokes plugins as ` --plugin-root-id= [plugin_args...]`. +// A bare positional token (no leading `--`) is treated as an explicit device path +// override, matching manual/standalone invocation. +struct ParsedArgs +{ + std::optional device_path; + std::string collection_id = "gamepad"; +}; + +ParsedArgs parse_args(int argc, char** argv) +{ + ParsedArgs parsed; + constexpr std::string_view kRootIdPrefix = "--plugin-root-id="; + for (int i = 1; i < argc; ++i) + { + const std::string_view arg = argv[i]; + if (arg.starts_with(kRootIdPrefix)) + { + parsed.collection_id = std::string(arg.substr(kRootIdPrefix.size())); + } + else if (!arg.starts_with("--")) + { + parsed.device_path = std::string(arg); + } + } + return parsed; +} + +} // namespace + +int main(int argc, char** argv) +try +{ + if (argc == 0) + { + std::cerr << "Usage: gamepad_plugin [device_path] [--plugin-root-id=]" << std::endl; + return 1; + } + + const ParsedArgs args = parse_args(argc, argv); + std::optional device_path = args.device_path; + if (!device_path) + device_path = discover_gamepad_device_path(); + if (!device_path) + { + std::cerr << argv[0] << ": No joystick device found under /dev/input/by-path/ and none given explicitly." + << std::endl; + return 1; + } + + std::cout << "Gamepad (device: " << *device_path << ", collection: " << args.collection_id << ")" << std::endl; + + GamepadPlugin plugin(*device_path, args.collection_id); + + // Push data at 90 Hz. + const auto frame_duration = std::chrono::nanoseconds(1000000000 / 90); + const auto program_start = std::chrono::steady_clock::now(); + std::size_t frame_count = 0; + + while (true) + { + plugin.update(); + frame_count++; + std::this_thread::sleep_until(program_start + frame_duration * frame_count); + } + + return 0; +} +catch (const std::exception& e) +{ + std::cerr << argv[0] << ": " << e.what() << std::endl; + return 1; +} +catch (...) +{ + std::cerr << argv[0] << ": Unknown error" << std::endl; + return 1; +} diff --git a/src/plugins/gamepad/plugin.yaml b/src/plugins/gamepad/plugin.yaml new file mode 100644 index 0000000000..53decaec3c --- /dev/null +++ b/src/plugins/gamepad/plugin.yaml @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: gamepad +description: "Raw gamepad button/axis state via Linux joystick device" +command: "./gamepad_plugin" +version: "1.0.0" +devices: + - path: "/gamepad" + type: "gamepad" + description: "Gamepad button/axis state from /dev/input/js*" diff --git a/src/python/isaacteleop/deviceio/__init__.py b/src/python/isaacteleop/deviceio/__init__.py index 0f9cc8c71c..ceb2ae2fb4 100644 --- a/src/python/isaacteleop/deviceio/__init__.py +++ b/src/python/isaacteleop/deviceio/__init__.py @@ -19,6 +19,7 @@ MessageChannelTracker, FrameMetadataTrackerOak, Generic3AxisPedalTracker, + GamepadTracker, OgloTactileTracker, TensorPushTracker, JointStateTracker, @@ -49,6 +50,7 @@ StreamType, FrameMetadataOak, Generic3AxisPedalOutput, + GamepadOutput, OgloGloveSample, ) @@ -60,6 +62,7 @@ "StreamType", "FrameMetadataOak", "Generic3AxisPedalOutput", + "GamepadOutput", "OgloGloveSample", "ITracker", "HandTracker", @@ -69,6 +72,7 @@ "MessageChannelTracker", "FrameMetadataTrackerOak", "Generic3AxisPedalTracker", + "GamepadTracker", "OgloTactileTracker", "TensorPushTracker", "JointStateTracker", diff --git a/src/python/isaacteleop/retargeters/__init__.py b/src/python/isaacteleop/retargeters/__init__.py index 316c9beacf..fee6f95485 100644 --- a/src/python/isaacteleop/retargeters/__init__.py +++ b/src/python/isaacteleop/retargeters/__init__.py @@ -16,6 +16,9 @@ - LocomotionRootCmdRetargeter: Locomotion from controller inputs - FootPedalRootCmdRetargeter: Root command from 3-axis foot pedal (horizontal/vertical + rudder) - GripperRetargeter: Pinch-based gripper control + - GamepadToSe3RelRetargeter: Gamepad stick/dpad state -> relative EE delta control + - GamepadGripperRetargeter: Gamepad X-button toggle -> gripper open/closed + - GamepadToSe2Retargeter: Gamepad stick state -> base velocity command (v_x, v_y, omega_z) - SO101ClutchRetargeter: Clutch-rebased absolute EE pose for the SO-101 5-DOF arm -- re-latches BOTH home position and orientation on every engage, base-frame left-composed, no fixed offset @@ -105,6 +108,33 @@ # .gripper_retargeter "GripperRetargeter": (".gripper_retargeter", "GripperRetargeter", None), "GripperRetargeterConfig": (".gripper_retargeter", "GripperRetargeterConfig", None), + # .gamepad_se3_retargeter (requires retargeters-lite extra: scipy) + "GamepadToSe3RelRetargeter": ( + ".gamepad_se3_retargeter", + "GamepadToSe3RelRetargeter", + "retargeters-lite", + ), + "GamepadToSe3RelRetargeterConfig": ( + ".gamepad_se3_retargeter", + "GamepadToSe3RelRetargeterConfig", + "retargeters-lite", + ), + "GamepadGripperRetargeter": ( + ".gamepad_se3_retargeter", + "GamepadGripperRetargeter", + "retargeters-lite", + ), + # .gamepad_se2_retargeter + "GamepadToSe2Retargeter": ( + ".gamepad_se2_retargeter", + "GamepadToSe2Retargeter", + None, + ), + "GamepadToSe2RetargeterConfig": ( + ".gamepad_se2_retargeter", + "GamepadToSe2RetargeterConfig", + None, + ), # .SO101 (SO-101 5-DOF arm: clutch EE-pose, analog gripper) "SO101ClutchRetargeter": ( ".SO101.clutch_retargeter", @@ -234,6 +264,11 @@ def __getattr__(name: str): # Manipulator retargeters "GripperRetargeter", "GripperRetargeterConfig", + "GamepadToSe3RelRetargeter", + "GamepadToSe3RelRetargeterConfig", + "GamepadGripperRetargeter", + "GamepadToSe2Retargeter", + "GamepadToSe2RetargeterConfig", # SO-101 5-DOF arm retargeters "SO101ClutchRetargeter", "SO101GripperRetargeter", diff --git a/src/python/isaacteleop/retargeters/gamepad_se2_retargeter.py b/src/python/isaacteleop/retargeters/gamepad_se2_retargeter.py new file mode 100644 index 0000000000..e576f42459 --- /dev/null +++ b/src/python/isaacteleop/retargeters/gamepad_se2_retargeter.py @@ -0,0 +1,92 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Gamepad SE2 Retargeter Module. + +Maps raw gamepad axis state to a base velocity command (v_x, v_y, omega_z). +""" + +from dataclasses import dataclass + +import numpy as np + +from isaacteleop.retargeting_engine.deviceio_source_nodes import GamepadAxesType +from isaacteleop.retargeting_engine.interface import ( + BaseRetargeter, + RetargeterIOType, +) +from isaacteleop.retargeting_engine.interface.retargeter_core_types import RetargeterIO +from isaacteleop.retargeting_engine.interface.tensor_group_type import ( + OptionalType, + TensorGroupType, +) +from isaacteleop.retargeting_engine.tensor_types import DLDataType, NDArrayType + +# Linux joystick-API axis indices for a typical Xbox-style pad under the xpad driver. +# Axis convention: pushing a stick left/up reports a negative value, right/down positive +# (standard HID convention). +AXIS_LEFT_X, AXIS_LEFT_Y = 0, 1 +AXIS_RIGHT_X = 3 + + +@dataclass +class GamepadToSe2RetargeterConfig: + """Configuration for the gamepad-to-SE2 base-velocity retargeter.""" + + v_x_sensitivity: float = 1.0 + v_y_sensitivity: float = 1.0 + omega_z_sensitivity: float = 1.0 + dead_zone: float = 0.01 + + +class GamepadToSe2Retargeter(BaseRetargeter): + """ + Maps gamepad stick state to a 3D base velocity command (v_x, v_y, omega_z). + + Stick bindings (matching Isaac Lab's legacy Se2Gamepad): + Left stick up/down: +/-v_x Left stick right/left: +/-v_y + Right stick right/left: +/-omega_z + + Output is the instantaneous command implied by the current stick deflection + (scaled by sensitivity), not an integrated velocity -- matching a continuous-axis + input device. + """ + + def __init__(self, config: GamepadToSe2RetargeterConfig, name: str) -> None: + self._config = config + super().__init__(name=name) + + def input_spec(self) -> RetargeterIOType: + return {"gamepad_axes": OptionalType(GamepadAxesType())} + + def output_spec(self) -> RetargeterIOType: + return { + "base_command": TensorGroupType( + "base_command", + [ + NDArrayType( + "velocity", shape=(3,), dtype=DLDataType.FLOAT, dtype_bits=32 + ) + ], + ) + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + base_command = outputs["base_command"] + axes_in = inputs["gamepad_axes"] + if axes_in.is_none: + base_command[0] = np.zeros(3, dtype=np.float32) + return + + axes = np.asarray(axes_in[0]) + dead_zone = self._config.dead_zone + + def deadzoned(value: float) -> float: + return 0.0 if abs(value) < dead_zone else value + + v_x = -deadzoned(axes[AXIS_LEFT_Y]) * self._config.v_x_sensitivity + v_y = deadzoned(axes[AXIS_LEFT_X]) * self._config.v_y_sensitivity + omega_z = deadzoned(axes[AXIS_RIGHT_X]) * self._config.omega_z_sensitivity + + base_command[0] = np.array([v_x, v_y, omega_z], dtype=np.float32) diff --git a/src/python/isaacteleop/retargeters/gamepad_se3_retargeter.py b/src/python/isaacteleop/retargeters/gamepad_se3_retargeter.py new file mode 100644 index 0000000000..b7c1a133c9 --- /dev/null +++ b/src/python/isaacteleop/retargeters/gamepad_se3_retargeter.py @@ -0,0 +1,168 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Gamepad SE3 Retargeter Module. + +Maps raw gamepad button/axis state to end-effector delta commands and a gripper toggle. +""" + +from dataclasses import dataclass + +import numpy as np +from scipy.spatial.transform import Rotation + +from isaacteleop.retargeting_engine.deviceio_source_nodes import ( + GamepadAxesType, + GamepadButtonsType, +) +from isaacteleop.retargeting_engine.interface import ( + BaseRetargeter, + RetargeterIOType, +) +from isaacteleop.retargeting_engine.interface.retargeter_core_types import RetargeterIO +from isaacteleop.retargeting_engine.interface.tensor_group_type import ( + OptionalType, + TensorGroupType, +) +from isaacteleop.retargeting_engine.tensor_types import ( + DLDataType, + FloatType, + NDArrayType, +) + +# Linux joystick-API axis indices for a typical Xbox-style pad under the xpad driver. +# Axis convention: pushing a stick left/up reports a negative value, right/down positive +# (standard HID convention). The D-pad is reported as a hat switch (axes 6/7) on most +# xpad-driver controllers rather than as buttons. +AXIS_LEFT_X, AXIS_LEFT_Y = 0, 1 +AXIS_RIGHT_X, AXIS_RIGHT_Y = 3, 4 +AXIS_DPAD_X, AXIS_DPAD_Y = 6, 7 + +# Typical xpad button ordering: A=0, B=1, X=2, Y=3, LB=4, RB=5, ... +BUTTON_X = 2 + + +@dataclass +class GamepadToSe3RelRetargeterConfig: + """Configuration for the gamepad-to-SE3-relative retargeter.""" + + pos_sensitivity: float = 0.4 + rot_sensitivity: float = 0.8 + dead_zone: float = 0.01 + + +class GamepadToSe3RelRetargeter(BaseRetargeter): + """ + Maps gamepad stick/dpad state to a 6D end-effector delta command. + + Stick/D-pad bindings (matching Isaac Lab's legacy Se3Gamepad): + Left stick up/down: +/-X, Left stick left/right: +/-Y, + Right stick up/down: +/-Z (position) + D-pad left/right: +/-roll, D-pad down/up: +/-pitch, + Right stick left/right: +/-yaw (rotation) + + Output is the instantaneous command implied by the current stick/dpad deflection + (scaled by sensitivity), not an integrated delta -- matching a continuous-axis + input device. + """ + + def __init__(self, config: GamepadToSe3RelRetargeterConfig, name: str) -> None: + self._config = config + super().__init__(name=name) + + def input_spec(self) -> RetargeterIOType: + return {"gamepad_axes": OptionalType(GamepadAxesType())} + + def output_spec(self) -> RetargeterIOType: + return { + "ee_delta": TensorGroupType( + "ee_delta", + [ + NDArrayType( + "delta", shape=(6,), dtype=DLDataType.FLOAT, dtype_bits=32 + ) + ], + ) + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + ee_delta = outputs["ee_delta"] + axes_in = inputs["gamepad_axes"] + if axes_in.is_none: + ee_delta[0] = np.zeros(6, dtype=np.float32) + return + + axes = np.asarray(axes_in[0]) + pos_sens = self._config.pos_sensitivity + rot_sens = self._config.rot_sensitivity + dead_zone = self._config.dead_zone + + def deadzoned(value: float) -> float: + return 0.0 if abs(value) < dead_zone else value + + delta_pos = np.zeros(3) + delta_pos[0] = -deadzoned(axes[AXIS_LEFT_Y]) * pos_sens + delta_pos[1] = -deadzoned(axes[AXIS_LEFT_X]) * pos_sens + delta_pos[2] = -deadzoned(axes[AXIS_RIGHT_Y]) * pos_sens + + delta_euler = np.zeros(3) + delta_euler[0] = -deadzoned(axes[AXIS_DPAD_X]) * rot_sens * 0.8 + delta_euler[1] = deadzoned(axes[AXIS_DPAD_Y]) * rot_sens * 0.8 + delta_euler[2] = -deadzoned(axes[AXIS_RIGHT_X]) * rot_sens + + delta_rot = Rotation.from_euler("XYZ", delta_euler).as_rotvec() + + ee_delta[0] = np.concatenate([delta_pos, delta_rot]).astype(np.float32) + + +class GamepadGripperRetargeter(BaseRetargeter): + """ + Toggles a gripper open/closed state on each rising edge of the X button. + + Output matches GripperRetargeter's convention: -1.0 when closed, 1.0 when open. + """ + + def __init__(self, name: str) -> None: + super().__init__(name=name) + self._closed = False + self._prev_x_pressed = False + + def input_spec(self) -> RetargeterIOType: + return {"gamepad_buttons": OptionalType(GamepadButtonsType())} + + def output_spec(self) -> RetargeterIOType: + return { + "gripper_command": TensorGroupType( + "gripper_command", [FloatType("command")] + ) + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + gripper_out = outputs["gripper_command"] + buttons_in = inputs["gamepad_buttons"] + x_pressed = ( + False if buttons_in.is_none else bool(np.asarray(buttons_in[0])[BUTTON_X]) + ) + + if context.execution_events.reset: + self._closed = False + # Sync to the current button state without toggling -- X may already be + # held on a reset frame, and that isn't a rising edge. Leave + # _prev_x_pressed alone when the device is inactive this frame; + # overwriting it to False would misread a still-held button as a fresh + # rising edge once data resumes. + if not buttons_in.is_none: + self._prev_x_pressed = x_pressed + gripper_out[0] = -1.0 if self._closed else 1.0 + return + + if buttons_in.is_none: + gripper_out[0] = -1.0 if self._closed else 1.0 + return + + if x_pressed and not self._prev_x_pressed: + self._closed = not self._closed + self._prev_x_pressed = x_pressed + + gripper_out[0] = -1.0 if self._closed else 1.0 diff --git a/src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/__init__.py b/src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/__init__.py index b1b583e572..e1e4496d80 100644 --- a/src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/__init__.py +++ b/src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/__init__.py @@ -11,6 +11,7 @@ from .hands_source import HandsSource from .controllers_source import ControllersSource from .pedals_source import Generic3AxisPedalSource +from .gamepad_source import GamepadAxesType, GamepadButtonsType, GamepadSource from .joint_state_source import JointStateSource from .full_body_source import FullBodySource from .message_channel_source import MessageChannelSource @@ -26,12 +27,14 @@ HandPoseTrackedType, ControllerSnapshotTrackedType, Generic3AxisPedalOutputTrackedType, + GamepadOutputTrackedType, JointStateOutputTrackedType, FullBodyPoseTrackedType, DeviceIOHeadPoseTracked, DeviceIOHandPoseTracked, DeviceIOControllerSnapshotTracked, DeviceIOGeneric3AxisPedalOutputTracked, + DeviceIOGamepadOutputTracked, DeviceIOJointStateOutputTracked, DeviceIOFullBodyPoseTracked, MessageChannelMessagesTrackedType, @@ -49,6 +52,9 @@ "HandsSource", "ControllersSource", "Generic3AxisPedalSource", + "GamepadAxesType", + "GamepadButtonsType", + "GamepadSource", "JointStateSource", "FullBodySource", "MessageChannelSource", @@ -61,6 +67,7 @@ "HandPoseTrackedType", "ControllerSnapshotTrackedType", "Generic3AxisPedalOutputTrackedType", + "GamepadOutputTrackedType", "JointStateOutputTrackedType", "FullBodyPoseTrackedType", "MessageChannelMessagesTrackedType", @@ -70,6 +77,7 @@ "DeviceIOHandPoseTracked", "DeviceIOControllerSnapshotTracked", "DeviceIOGeneric3AxisPedalOutputTracked", + "DeviceIOGamepadOutputTracked", "DeviceIOJointStateOutputTracked", "DeviceIOFullBodyPoseTracked", "DeviceIOMessageChannelMessagesTracked", diff --git a/src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py b/src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py index e06eaa6777..12456890ff 100644 --- a/src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py +++ b/src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py @@ -19,6 +19,7 @@ HandPose, ControllerSnapshot, Generic3AxisPedalOutput, + GamepadOutput, JointStateOutput, FullBodyPose, MessageChannelMessagesTracked, @@ -92,6 +93,12 @@ class Generic3AxisPedalOutputTrackedType(_PayloadTensorType): _payload_cls = Generic3AxisPedalOutput +class GamepadOutputTrackedType(_PayloadTensorType): + """GamepadOutput payload from DeviceIO GamepadTracker.""" + + _payload_cls = GamepadOutput + + class JointStateOutputTrackedType(_PayloadTensorType): """JointStateOutput payload from DeviceIO JointStateTracker.""" @@ -172,6 +179,18 @@ def DeviceIOGeneric3AxisPedalOutputTracked() -> TensorGroupType: ) +def DeviceIOGamepadOutputTracked() -> TensorGroupType: + """Tracked gamepad data from DeviceIO GamepadTracker. + + Contains: + gamepad_tracked: GamepadOutput handle, or None when inactive + """ + return TensorGroupType( + "deviceio_gamepad_output", + [GamepadOutputTrackedType("gamepad_tracked")], + ) + + def DeviceIOJointStateOutputTracked() -> TensorGroupType: """Tracked joint-state data from DeviceIO JointStateTracker. diff --git a/src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/gamepad_source.py b/src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/gamepad_source.py new file mode 100644 index 0000000000..cfa1e86214 --- /dev/null +++ b/src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/gamepad_source.py @@ -0,0 +1,179 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Gamepad Source Node - DeviceIO to Retargeting Engine converter. + +Converts raw GamepadOutput flatbuffer data (Linux joystick-API button/axis state) to +two standard outputs: a button-press bitmap and an axis-value array. Carries no +semantic mapping -- which button/axis means what (a stick, a trigger, a toggle) is +entirely up to the consuming retargeter. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any + +from ..interface.retargeter_core_types import RetargeterIO, RetargeterIOType +from ..interface.tensor_group import TensorGroup +from ..interface.tensor_group_type import OptionalType, TensorGroupType +from ..tensor_types import DLDataType, NDArrayType +from .deviceio_tensor_types import DeviceIOGamepadOutputTracked +from .interface import IDeviceIOSource + +if TYPE_CHECKING: + from isaacteleop.deviceio import ITracker + from isaacteleop.schema import GamepadOutput + +# Default collection_id matching the gamepad plugin and GamepadTracker. +DEFAULT_GAMEPAD_COLLECTION_ID = "gamepad" + +# Linux joystick JS_EVENT_BUTTON indices go up to 31 on every driver observed in +# practice (Xbox-style pads report ~11); 32 covers the full range with headroom. +GAMEPAD_BUTTONS_BITMAP_SIZE = 32 + +# Fixed axis-array size returned to consumers, independent of how many axes the +# connected device actually reports (GamepadPlugin queries JSIOCGAXES and reports +# fewer/more; this source pads with 0.0 or truncates to fit). +GAMEPAD_AXES_SIZE = 8 + + +def GamepadButtonsType() -> TensorGroupType: + """Type for the "gamepad_buttons" output: a 32-entry uint8 bitmap indexed by joystick button number.""" + return TensorGroupType( + "gamepad_buttons", + [ + NDArrayType( + "bitmap", + shape=(GAMEPAD_BUTTONS_BITMAP_SIZE,), + dtype=DLDataType.UINT, + dtype_bits=8, + ) + ], + ) + + +def GamepadAxesType() -> TensorGroupType: + """Type for the "gamepad_axes" output: a fixed-size float32 array of joystick axis values.""" + return TensorGroupType( + "gamepad_axes", + [ + NDArrayType( + "axes", + shape=(GAMEPAD_AXES_SIZE,), + dtype=DLDataType.FLOAT, + dtype_bits=32, + ) + ], + ) + + +class GamepadSource(IDeviceIOSource): + """ + Stateless converter: DeviceIO GamepadOutput → button-bitmap / axis-array tensors. + + Inputs: + - "deviceio_gamepad": Raw GamepadOutput flatbuffer from GamepadTracker + + Outputs (Optional — absent when the gamepad plugin has not yet streamed): + - "gamepad_buttons": OptionalTensorGroup, a 32-entry uint8 bitmap indexed by + Linux joystick button number (1 = held, 0 = released). + - "gamepad_axes": OptionalTensorGroup, a fixed-size float32 array of axis + values in [-1, 1], padded/truncated to a fixed length independent of the + connected device's actual axis count. + + Usage: + # In TeleopSession, the gamepad tracker is discovered from the pipeline; + # data is polled via poll_tracker. Or manually: + tracked = gamepad_tracker.get_gamepad_data(session) + result = gamepad_source_node({ + "deviceio_gamepad": TensorGroup(DeviceIOGamepadOutputTracked(), [tracked]) + }) + """ + + def __init__( + self, name: str, collection_id: str = DEFAULT_GAMEPAD_COLLECTION_ID + ) -> None: + """Initialize stateless gamepad source node. + + Creates a GamepadTracker instance for TeleopSession to discover and use. + + Args: + name: Unique name for this source node + collection_id: Tensor collection ID for gamepad data (must match the gamepad plugin). + """ + import isaacteleop.deviceio as deviceio + + self._gamepad_tracker = deviceio.GamepadTracker(collection_id) + self._collection_id = collection_id + super().__init__(name) + + def get_tracker(self) -> ITracker: + """Get the GamepadTracker instance. + + Returns: + The GamepadTracker instance for TeleopSession to initialize + """ + return self._gamepad_tracker + + def poll_tracker(self, deviceio_session: Any) -> RetargeterIO: + """Poll the gamepad tracker and return input data. + + Args: + deviceio_session: The active DeviceIO session. + + Returns: + Dict with "deviceio_gamepad" TensorGroup containing GamepadOutput | None. + """ + state = self._gamepad_tracker.get_gamepad_data(deviceio_session) + tg = TensorGroup(DeviceIOGamepadOutputTracked()) + tg[0] = state + return {"deviceio_gamepad": tg} + + def input_spec(self) -> RetargeterIOType: + """Declare DeviceIO gamepad input.""" + return { + "deviceio_gamepad": DeviceIOGamepadOutputTracked(), + } + + def output_spec(self) -> RetargeterIOType: + """Declare standard gamepad outputs (Optional — may be absent).""" + return { + "gamepad_buttons": OptionalType(GamepadButtonsType()), + "gamepad_axes": OptionalType(GamepadAxesType()), + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + """ + Convert DeviceIO GamepadOutput to the standard gamepad outputs. + + Calls ``set_none()`` on both outputs when the gamepad plugin has not yet + streamed. + + Args: + inputs: Dict with "deviceio_gamepad" containing GamepadOutput | None + outputs: Dict with "gamepad_buttons" and "gamepad_axes" OptionalTensorGroups + context: Shared ComputeContext for the current step (carries GraphTime). + """ + import numpy as np + + state: GamepadOutput | None = inputs["deviceio_gamepad"][0] + + buttons_out = outputs["gamepad_buttons"] + axes_out = outputs["gamepad_axes"] + if state is None: + buttons_out.set_none() + axes_out.set_none() + return + + bitmap = np.zeros(GAMEPAD_BUTTONS_BITMAP_SIZE, dtype=np.uint8) + for code in state.pressed_buttons: + if code < GAMEPAD_BUTTONS_BITMAP_SIZE: + bitmap[code] = 1 + buttons_out[0] = bitmap + + axes = np.zeros(GAMEPAD_AXES_SIZE, dtype=np.float32) + reported = np.asarray(state.axes, dtype=np.float32) + count = min(reported.shape[0], GAMEPAD_AXES_SIZE) + axes[:count] = reported[:count] + axes_out[0] = axes diff --git a/src/python/isaacteleop/schema/__init__.py b/src/python/isaacteleop/schema/__init__.py index 39829651d5..8190f41305 100644 --- a/src/python/isaacteleop/schema/__init__.py +++ b/src/python/isaacteleop/schema/__init__.py @@ -38,6 +38,9 @@ # Pedals-related types. Generic3AxisPedalOutput, Generic3AxisPedalOutputRecord, + # Gamepad types (raw joystick-API button/axis state). + GamepadOutput, + GamepadOutputRecord, # OGLO tactile glove types. OgloGloveSample, OgloGloveSampleRecord, @@ -123,6 +126,9 @@ def __getattr__(name: str): # Pedals types. "Generic3AxisPedalOutput", "Generic3AxisPedalOutputRecord", + # Gamepad types (raw joystick-API button/axis state). + "GamepadOutput", + "GamepadOutputRecord", # OGLO tactile glove types. "OgloGloveSample", "OgloGloveSampleRecord", diff --git a/tests/python/core/retargeting_engine/test_gamepad_retargeter.py b/tests/python/core/retargeting_engine/test_gamepad_retargeter.py new file mode 100644 index 0000000000..0cf6ebafce --- /dev/null +++ b/tests/python/core/retargeting_engine/test_gamepad_retargeter.py @@ -0,0 +1,280 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Sim-free unit tests for GamepadToSe3RelRetargeter, GamepadGripperRetargeter, and +GamepadToSe2Retargeter, exercised through GamepadSource so a regression anywhere in the +schema -> source -> retargeter chain (a field rename, an index drift, a sign flip) fails +here, with no OpenXR device involved. +""" + +import numpy as np +import pytest + +from isaacteleop.retargeting_engine.deviceio_source_nodes import GamepadSource +from isaacteleop.retargeting_engine.interface.base_retargeter import _make_output_group +from isaacteleop.retargeting_engine.interface.execution_events import ExecutionEvents +from isaacteleop.retargeting_engine.interface.retargeter_core_types import ( + ComputeContext, +) +from isaacteleop.retargeting_engine.interface.tensor_group import TensorGroup +from isaacteleop.retargeters import ( + GamepadGripperRetargeter, + GamepadToSe2Retargeter, + GamepadToSe2RetargeterConfig, + GamepadToSe3RelRetargeter, + GamepadToSe3RelRetargeterConfig, +) +from isaacteleop.schema import GamepadOutput + +# Linux joystick-API axis indices, matching gamepad_plugin.cpp / GamepadSource / the retargeters. +AXIS_LEFT_X, AXIS_LEFT_Y = 0, 1 +AXIS_RIGHT_X, AXIS_RIGHT_Y = 3, 4 +AXIS_DPAD_X, AXIS_DPAD_Y = 6, 7 +BUTTON_X = 2 + + +def _axes(by_index: dict[int, float]) -> list[float]: + values = [0.0] * 8 + for index, value in by_index.items(): + values[index] = value + return values + + +def _gamepad_source(): + return GamepadSource(name="gamepad") + + +def _run_source( + src, pressed_buttons: list[int] | None, axes: list[float] | None = None +): + """Feed raw button/axis state (None = inactive device) through GamepadSource.compute().""" + state = ( + None + if pressed_buttons is None + else GamepadOutput(pressed_buttons, axes or [0.0] * 8, True) + ) + + input_spec = src.input_spec() + tg = TensorGroup(input_spec["deviceio_gamepad"]) + tg[0] = state + + outputs = {name: _make_output_group(gt) for name, gt in src.output_spec().items()} + src.compute({"deviceio_gamepad": tg}, outputs) + return outputs + + +class TestGamepadToSe3RelRetargeter: + def test_left_stick_up_produces_forward_delta(self): + """Left stick pushed up (axis Y = -1) -> GamepadSource -> Se3Retargeter -> +X delta.""" + src = _gamepad_source() + src_outputs = _run_source(src, [], axes=_axes({AXIS_LEFT_Y: -1.0})) + assert not src_outputs["gamepad_axes"].is_none + + retargeter = GamepadToSe3RelRetargeter( + GamepadToSe3RelRetargeterConfig(), name="se3" + ) + out = {"ee_delta": _make_output_group(retargeter.output_spec()["ee_delta"])} + retargeter.compute({"gamepad_axes": src_outputs["gamepad_axes"]}, out) + + delta = np.asarray(out["ee_delta"][0]) + assert delta[0] == pytest.approx(0.4) # default pos_sensitivity + assert np.allclose(delta[1:], 0.0) + + def test_opposing_axes_combine(self): + """Left stick up (+X) and right stick up (+Z) held together combine on independent axes.""" + src = _gamepad_source() + axes = _axes({AXIS_LEFT_Y: -1.0, AXIS_RIGHT_Y: -1.0}) + src_outputs = _run_source(src, [], axes=axes) + + retargeter = GamepadToSe3RelRetargeter( + GamepadToSe3RelRetargeterConfig(), name="se3" + ) + out = {"ee_delta": _make_output_group(retargeter.output_spec()["ee_delta"])} + retargeter.compute({"gamepad_axes": src_outputs["gamepad_axes"]}, out) + + delta = np.asarray(out["ee_delta"][0]) + assert delta[0] == pytest.approx(0.4) # left stick up: +X + assert delta[2] == pytest.approx(0.4) # right stick up: +Z + assert delta[1] == pytest.approx(0.0) + assert np.allclose(delta[3:], 0.0) # no rotation axes deflected + + def test_inactive_device_yields_zero_delta(self): + src = _gamepad_source() + src_outputs = _run_source(src, None) + + se3 = GamepadToSe3RelRetargeter(GamepadToSe3RelRetargeterConfig(), name="se3") + se3_out = {"ee_delta": _make_output_group(se3.output_spec()["ee_delta"])} + se3.compute({"gamepad_axes": src_outputs["gamepad_axes"]}, se3_out) + assert np.allclose(np.asarray(se3_out["ee_delta"][0]), 0.0) + + def test_dead_zone_suppresses_small_deflection(self): + """A deflection smaller than the configured dead zone is treated as zero.""" + src = _gamepad_source() + axes = _axes({AXIS_LEFT_Y: -0.005}) + src_outputs = _run_source(src, [], axes=axes) + + retargeter = GamepadToSe3RelRetargeter( + GamepadToSe3RelRetargeterConfig(), name="se3" + ) + out = {"ee_delta": _make_output_group(retargeter.output_spec()["ee_delta"])} + retargeter.compute({"gamepad_axes": src_outputs["gamepad_axes"]}, out) + + assert np.allclose(np.asarray(out["ee_delta"][0]), 0.0) + + +class TestGamepadGripperRetargeter: + def test_gripper_toggles_on_button_rising_edge_only(self): + """X press/release/press across three frames toggles exactly on each rising edge.""" + src = _gamepad_source() + retargeter = GamepadGripperRetargeter(name="gripper") + + def step(pressed_buttons): + src_outputs = _run_source(src, pressed_buttons) + out = { + "gripper_command": _make_output_group( + retargeter.output_spec()["gripper_command"] + ) + } + retargeter.compute({"gamepad_buttons": src_outputs["gamepad_buttons"]}, out) + return float(out["gripper_command"][0]) + + assert step([]) == pytest.approx(1.0) # open (default) + assert step([BUTTON_X]) == pytest.approx(-1.0) # rising edge -> close + assert step([BUTTON_X]) == pytest.approx( + -1.0 + ) # held -> stays closed, no re-toggle + assert step([]) == pytest.approx(-1.0) # release -> stays closed + assert step([BUTTON_X]) == pytest.approx(1.0) # rising edge again -> open + + def test_inactive_device_yields_default_open(self): + src = _gamepad_source() + src_outputs = _run_source(src, None) + + gripper = GamepadGripperRetargeter(name="gripper") + gripper_out = { + "gripper_command": _make_output_group( + gripper.output_spec()["gripper_command"] + ) + } + gripper.compute( + {"gamepad_buttons": src_outputs["gamepad_buttons"]}, gripper_out + ) + assert float(gripper_out["gripper_command"][0]) == pytest.approx( + 1.0 + ) # default open + + def test_reset_does_not_toggle_gripper_while_x_is_held(self): + """X held across a reset frame is not a rising edge and must not toggle the gripper.""" + src = _gamepad_source() + retargeter = GamepadGripperRetargeter(name="gripper") + + def step(pressed_buttons, reset=False): + src_outputs = _run_source(src, pressed_buttons) + out = { + "gripper_command": _make_output_group( + retargeter.output_spec()["gripper_command"] + ) + } + context = ComputeContext(execution_events=ExecutionEvents(reset=reset)) + retargeter.compute( + {"gamepad_buttons": src_outputs["gamepad_buttons"]}, out, context + ) + return float(out["gripper_command"][0]) + + assert step([BUTTON_X]) == pytest.approx(-1.0) # rising edge -> close + # Reset resets the gripper to open, but X is still held -- not a new rising + # edge, so this must not immediately re-close it. + assert step([BUTTON_X], reset=True) == pytest.approx(1.0) + assert step([BUTTON_X]) == pytest.approx(1.0) # still held -> stays open + assert step([]) == pytest.approx(1.0) # release + assert step([BUTTON_X]) == pytest.approx(-1.0) # genuine rising edge -> close + + def test_reset_with_inactive_device_preserves_prior_edge_state(self): + """A reset frame with no gamepad data must not clobber _prev_x_pressed.""" + src = _gamepad_source() + retargeter = GamepadGripperRetargeter(name="gripper") + + def step(pressed_buttons, reset=False): + src_outputs = _run_source(src, pressed_buttons) + out = { + "gripper_command": _make_output_group( + retargeter.output_spec()["gripper_command"] + ) + } + context = ComputeContext(execution_events=ExecutionEvents(reset=reset)) + retargeter.compute( + {"gamepad_buttons": src_outputs["gamepad_buttons"]}, out, context + ) + return float(out["gripper_command"][0]) + + assert step([BUTTON_X]) == pytest.approx(-1.0) # rising edge -> close + # Reset while the device is inactive (gamepad_buttons.is_none) -- must not + # force _prev_x_pressed to False, or the next frame (X still held) would be + # misread as a fresh rising edge. + assert step(None, reset=True) == pytest.approx(1.0) # gripper still resets + assert step([BUTTON_X]) == pytest.approx( + 1.0 + ) # still held -> no spurious toggle + + def test_other_button_does_not_affect_gripper(self): + """A button other than X shows up in gamepad_buttons but does not affect the gripper.""" + src = _gamepad_source() + src_outputs = _run_source(src, [5]) # RB, not the gripper button + + gripper = GamepadGripperRetargeter(name="gripper") + gripper_out = { + "gripper_command": _make_output_group( + gripper.output_spec()["gripper_command"] + ) + } + gripper.compute( + {"gamepad_buttons": src_outputs["gamepad_buttons"]}, gripper_out + ) + assert float(gripper_out["gripper_command"][0]) == pytest.approx( + 1.0 + ) # unaffected + + +class TestGamepadToSe2Retargeter: + def test_left_and_right_stick_combine(self): + """Left-stick-up (+v_x) and right-stick-right (+omega_z) held together -> combined base_command.""" + src = _gamepad_source() + axes = _axes({AXIS_LEFT_Y: -1.0, AXIS_RIGHT_X: 0.5}) + src_outputs = _run_source(src, [], axes=axes) + + retargeter = GamepadToSe2Retargeter(GamepadToSe2RetargeterConfig(), name="se2") + out = { + "base_command": _make_output_group(retargeter.output_spec()["base_command"]) + } + retargeter.compute({"gamepad_axes": src_outputs["gamepad_axes"]}, out) + + velocity = np.asarray(out["base_command"][0]) + assert velocity[0] == pytest.approx(1.0) # default v_x_sensitivity + assert velocity[1] == pytest.approx(0.0) + assert velocity[2] == pytest.approx(0.5) # default omega_z_sensitivity + + def test_dead_zone_suppresses_small_deflection(self): + """A deflection smaller than the configured dead zone is treated as zero.""" + src = _gamepad_source() + axes = _axes({AXIS_LEFT_Y: -0.005}) + src_outputs = _run_source(src, [], axes=axes) + + retargeter = GamepadToSe2Retargeter(GamepadToSe2RetargeterConfig(), name="se2") + out = { + "base_command": _make_output_group(retargeter.output_spec()["base_command"]) + } + retargeter.compute({"gamepad_axes": src_outputs["gamepad_axes"]}, out) + + assert np.allclose(np.asarray(out["base_command"][0]), 0.0) + + def test_inactive_device_yields_zero_velocity(self): + src = _gamepad_source() + src_outputs = _run_source(src, None) + + retargeter = GamepadToSe2Retargeter(GamepadToSe2RetargeterConfig(), name="se2") + out = { + "base_command": _make_output_group(retargeter.output_spec()["base_command"]) + } + retargeter.compute({"gamepad_axes": src_outputs["gamepad_axes"]}, out) + + assert np.allclose(np.asarray(out["base_command"][0]), 0.0) diff --git a/tests/python/core/retargeting_engine/test_gamepad_source.py b/tests/python/core/retargeting_engine/test_gamepad_source.py new file mode 100644 index 0000000000..f9fb165aa2 --- /dev/null +++ b/tests/python/core/retargeting_engine/test_gamepad_source.py @@ -0,0 +1,75 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Tests for the GamepadSource DeviceIO converter. + +Exercises the stateless converter from a raw ``GamepadOutput`` FlatBuffer (constructed via +the real schema Python bindings) into a button-press bitmap and a fixed-size axis array, +with no OpenXR device involved. +""" + +import numpy as np + +from isaacteleop.retargeting_engine.deviceio_source_nodes import GamepadSource +from isaacteleop.retargeting_engine.interface.base_retargeter import _make_output_group +from isaacteleop.retargeting_engine.interface.tensor_group import TensorGroup +from isaacteleop.schema import GamepadOutput + +AXIS_LEFT_Y = 1 +BUTTON_X = 2 + + +def _gamepad_source(): + return GamepadSource(name="gamepad") + + +def _run_source( + src, pressed_buttons: list[int] | None, axes: list[float] | None = None +): + """Feed raw button/axis state (None = inactive device) through GamepadSource.compute().""" + state = ( + None + if pressed_buttons is None + else GamepadOutput(pressed_buttons, axes or [], True) + ) + + input_spec = src.input_spec() + tg = TensorGroup(input_spec["deviceio_gamepad"]) + tg[0] = state + + outputs = {name: _make_output_group(gt) for name, gt in src.output_spec().items()} + src.compute({"deviceio_gamepad": tg}, outputs) + return outputs + + +class TestGamepadSource: + def test_source_creates_real_tracker(self): + src = _gamepad_source() + tracker = src.get_tracker() + assert tracker is not None + assert tracker.get_name() == "GamepadTracker" + + def test_button_marks_bitmap(self): + src = _gamepad_source() + outputs = _run_source(src, [BUTTON_X]) + + assert not outputs["gamepad_buttons"].is_none + bitmap = np.asarray(outputs["gamepad_buttons"][0]) + assert bitmap[BUTTON_X] == 1 + assert bitmap.sum() == 1 + + def test_axes_pad_to_fixed_size(self): + src = _gamepad_source() + axes = [0.0] * 8 + axes[AXIS_LEFT_Y] = -1.0 + outputs = _run_source(src, [], axes=axes) + + assert not outputs["gamepad_axes"].is_none + reported = np.asarray(outputs["gamepad_axes"][0]) + assert reported[AXIS_LEFT_Y] == -1.0 + + def test_inactive_device_yields_none(self): + src = _gamepad_source() + outputs = _run_source(src, None) + assert outputs["gamepad_buttons"].is_none + assert outputs["gamepad_axes"].is_none diff --git a/tests/python/core/schema/test_gamepad.py b/tests/python/core/schema/test_gamepad.py new file mode 100644 index 0000000000..840b1ec76b --- /dev/null +++ b/tests/python/core/schema/test_gamepad.py @@ -0,0 +1,104 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Unit tests for GamepadOutput type in isaacteleop.schema. + +Tests the following FlatBuffers types: +- GamepadOutput: Table with pressed_buttons (joystick button indices) and axes +- GamepadOutputRecord: Record wrapper carrying DeviceDataTimestamp + +Timestamps are carried by GamepadOutputRecord, not GamepadOutput. +""" + +import pytest + +from isaacteleop.schema import DeviceDataTimestamp, GamepadOutput, GamepadOutputRecord + + +class TestGamepadOutputConstruction: + """Tests for GamepadOutput table construction.""" + + def test_construction(self): + """Test construction with explicit fields.""" + output = GamepadOutput(pressed_buttons=[2], axes=[0.5, -0.5], is_valid=True) + + assert list(output.pressed_buttons) == [2] + assert list(output.axes) == pytest.approx([0.5, -0.5]) + assert output.is_valid is True + + def test_repr(self): + """Test __repr__ returns meaningful string.""" + output = GamepadOutput(pressed_buttons=[], axes=[], is_valid=False) + repr_str = repr(output) + + assert "GamepadOutput" in repr_str + + +class TestGamepadOutputFields: + """Tests that pressed_buttons and axes round-trip through the encoding.""" + + def test_empty_fields(self): + """Test encoding with no buttons held and no axes reported.""" + output = GamepadOutput(pressed_buttons=[], axes=[], is_valid=True) + + assert list(output.pressed_buttons) == [] + assert list(output.axes) == [] + + def test_multiple_pressed_buttons(self): + """Test encoding multiple simultaneously-held buttons.""" + output = GamepadOutput(pressed_buttons=[0, 5], axes=[], is_valid=True) + + assert list(output.pressed_buttons) == [0, 5] + + def test_encodings_are_independent(self): + """Test each encoding carries its own values, not a shared buffer's.""" + first = GamepadOutput(pressed_buttons=[0], axes=[1.0], is_valid=True) + second = GamepadOutput(pressed_buttons=[1], axes=[-1.0], is_valid=True) + + assert list(first.pressed_buttons) == [0] + assert list(second.pressed_buttons) == [1] + assert list(first.axes) == pytest.approx([1.0]) + assert list(second.axes) == pytest.approx([-1.0]) + + +class TestGamepadOutputEncoding: + """Tests that an encoded payload reads back. + + A tracker with no gamepad data returns None rather than an empty payload, so + absence needs no case here; the source-node tests cover feeding None through. + """ + + def test_encoded_payload_reads_back(self): + """An encoded payload gates as True and its fields read directly.""" + output = GamepadOutput(pressed_buttons=[2], axes=[0.5], is_valid=True) + + assert output + assert list(output.pressed_buttons) == [2] + assert list(output.axes) == pytest.approx([0.5]) + + def test_repr_present(self): + """Repr of a present payload names the type.""" + assert "GamepadOutput" in repr( + GamepadOutput(pressed_buttons=[], axes=[], is_valid=True) + ) + + +class TestGamepadOutputRecordTimestamp: + """Tests for GamepadOutputRecord with DeviceDataTimestamp.""" + + def test_construction_with_timestamp(self): + """Test GamepadOutputRecord carries DeviceDataTimestamp.""" + data = GamepadOutput(pressed_buttons=[2], axes=[0.5], is_valid=True) + ts = DeviceDataTimestamp(1000000000, 2000000000, 3000000000) + record = GamepadOutputRecord(data, ts) + + assert record.timestamp.available_time_local_common_clock == 1000000000 + assert record.timestamp.sample_time_local_common_clock == 2000000000 + assert record.timestamp.sample_time_raw_device_clock == 3000000000 + assert list(record.data.pressed_buttons) == [2] + + def test_payload_less_record(self): + """A record may carry a timestamp and no payload: MCAP's frame sentinel.""" + record = GamepadOutputRecord(None, DeviceDataTimestamp(1, 2, 3)) + assert record.data is None + assert record.timestamp.available_time_local_common_clock == 1