diff --git a/CMakeLists.txt b/CMakeLists.txt index c5d565cc04..3ed926a255 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -186,6 +186,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) @@ -199,5 +200,23 @@ if(BUILD_PLUGINS) endif() endif() +# Bundle the gamepad plugin binary + metadata inside the wheel (isaacteleop.plugins. +# gamepad) so `pip install isaacteleop` alone is sufficient to run it via +# PluginManager -- no separate `cmake --install` step required. A standalone target +# (rather than baked into python_package's own COMMAND list in +# src/core/python/CMakeLists.txt) because src/core is added before src/plugins, so +# `gamepad_plugin` isn't defined yet at that point in the configure. +if(CMAKE_SYSTEM_NAME STREQUAL "Linux" AND TARGET gamepad_plugin AND TARGET python_package) + add_custom_target(bundle_gamepad_plugin ALL + COMMAND ${CMAKE_COMMAND} -E make_directory "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/plugins/gamepad" + COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_SOURCE_DIR}/src/core/python/isaacteleop_plugins_init.py" "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/plugins/__init__.py" + COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_SOURCE_DIR}/src/core/python/isaacteleop_plugins_gamepad_init.py" "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/plugins/gamepad/__init__.py" + COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_SOURCE_DIR}/src/plugins/gamepad/plugin.yaml" "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/plugins/gamepad/plugin.yaml" + COMMAND ${CMAKE_COMMAND} -E copy "$" "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/plugins/gamepad/gamepad_plugin" + DEPENDS gamepad_plugin python_package + COMMENT "Bundling gamepad plugin into the Python package" + ) +endif() + # Formatting enforcement (runs on Linux by default) include(cmake/ClangFormat.cmake) diff --git a/examples/teleop/python/gamepad_printer_example.py b/examples/teleop/python/gamepad_printer_example.py new file mode 100644 index 0000000000..6fc7fa9579 --- /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 isaacteleop.cloudxr import CloudXRLauncher +from isaacteleop.plugins import plugin_search_path +from isaacteleop.retargeting_engine.deviceio_source_nodes import GamepadSource +from isaacteleop.teleop_session_manager import ( + TeleopSession, + TeleopSessionConfig, + PluginConfig, +) + + +PLUGIN_ROOT_DIR = plugin_search_path() +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_base/cpp/inc/deviceio_base/gamepad_tracker_base.hpp b/src/core/deviceio_base/cpp/inc/deviceio_base/gamepad_tracker_base.hpp new file mode 100644 index 0000000000..ac27aaec57 --- /dev/null +++ b/src/core/deviceio_base/cpp/inc/deviceio_base/gamepad_tracker_base.hpp @@ -0,0 +1,20 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "tracker.hpp" + +namespace core +{ + +struct GamepadOutputTrackedT; + +// Abstract base interface for GamepadTracker implementations. +class IGamepadTrackerImpl : public ITrackerImpl +{ +public: + virtual const GamepadOutputTrackedT& get_data() const = 0; +}; + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/CMakeLists.txt b/src/core/deviceio_trackers/cpp/CMakeLists.txt index 1b432d7e98..5772397ef9 100644 --- a/src/core/deviceio_trackers/cpp/CMakeLists.txt +++ b/src/core/deviceio_trackers/cpp/CMakeLists.txt @@ -10,6 +10,7 @@ add_library(deviceio_trackers STATIC controller_tracker.cpp message_channel_tracker.cpp generic_3axis_pedal_tracker.cpp + gamepad_tracker.cpp oglo_tactile_tracker.cpp tensor_push_tracker.cpp haptic_command_reader_tracker.cpp @@ -23,6 +24,7 @@ add_library(deviceio_trackers STATIC inc/deviceio_trackers/message_channel_tracker.hpp inc/deviceio_trackers/full_body_tracker.hpp inc/deviceio_trackers/generic_3axis_pedal_tracker.hpp + inc/deviceio_trackers/gamepad_tracker.hpp inc/deviceio_trackers/oglo_tactile_tracker.hpp inc/deviceio_trackers/tensor_push_tracker.hpp inc/deviceio_trackers/haptic_command_reader_tracker.hpp diff --git a/src/core/deviceio_trackers/cpp/gamepad_tracker.cpp b/src/core/deviceio_trackers/cpp/gamepad_tracker.cpp new file mode 100644 index 0000000000..8de3d11c4e --- /dev/null +++ b/src/core/deviceio_trackers/cpp/gamepad_tracker.cpp @@ -0,0 +1,23 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/deviceio_trackers/gamepad_tracker.hpp" + +namespace core +{ + +// ============================================================================ +// GamepadTracker +// ============================================================================ + +GamepadTracker::GamepadTracker(const std::string& collection_id, size_t max_flatbuffer_size) + : collection_id_(collection_id), max_flatbuffer_size_(max_flatbuffer_size) +{ +} + +const GamepadOutputTrackedT& GamepadTracker::get_data(const ITrackerSession& session) const +{ + return static_cast(session.get_tracker_impl(*this)).get_data(); +} + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/gamepad_tracker.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/gamepad_tracker.hpp new file mode 100644 index 0000000000..367c17c452 --- /dev/null +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/gamepad_tracker.hpp @@ -0,0 +1,78 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +#include +#include + +namespace core +{ + +/*! + * @brief Facade for raw joystick-API button/axis state exposed as ``GamepadOutputTrackedT``. + * + * Semantic contract: ``pressed_buttons`` is the set of currently-held Linux joystick + * button indices, ``axes`` is the current value of every reported axis; no semantic + * mapping (stick, trigger, toggle, ...) is applied here. After each + * ``ITrackerSession::update()`` that includes this tracker, ``get_data(session)`` + * reflects the implementation's tracked snapshot. **Absent** data (``data`` null) + * means no sample has been unpacked yet or the collection/source is unavailable. + * + * Usage: + * @code + * auto tracker = std::make_shared("gamepad"); + * // ... register the tracker with a session, then each tick: ... + * session->update(); + * const auto& data = tracker->get_data(*session); + * @endcode + */ +class GamepadTracker : public ITracker +{ +public: + //! Default maximum FlatBuffer size for GamepadOutput messages. + static constexpr size_t DEFAULT_MAX_FLATBUFFER_SIZE = 512; + + /*! + * @brief Constructs a GamepadTracker. + * @param collection_id Logical stream identifier; must match the data source for the chosen backend + * (see live implementation documentation). + * @param max_flatbuffer_size Upper bound for serialized ``GamepadOutput`` / record payloads + * (default: 512 bytes); must be sufficient for the schema and backend. + */ + explicit GamepadTracker(const std::string& collection_id, size_t max_flatbuffer_size = DEFAULT_MAX_FLATBUFFER_SIZE); + + std::string_view get_name() const override + { + return TRACKER_NAME; + } + + /*! + * @brief Gamepad snapshot from the session's implementation. + * + * ``tracked.data`` is null when there is no valid last-known sample (source never + * provided data or implementation cleared state when the collection is gone). + */ + const GamepadOutputTrackedT& get_data(const ITrackerSession& session) const; + + const std::string& collection_id() const + { + return collection_id_; + } + + size_t max_flatbuffer_size() const + { + return max_flatbuffer_size_; + } + +private: + static constexpr const char* TRACKER_NAME = "GamepadTracker"; + + std::string collection_id_; + size_t max_flatbuffer_size_; +}; + +} // namespace core diff --git a/src/core/deviceio_trackers/python/deviceio_trackers_init.py b/src/core/deviceio_trackers/python/deviceio_trackers_init.py index 2cdafa3e18..c4c27fe70a 100644 --- a/src/core/deviceio_trackers/python/deviceio_trackers_init.py +++ b/src/core/deviceio_trackers/python/deviceio_trackers_init.py @@ -14,6 +14,7 @@ MessageChannelTracker, FrameMetadataTrackerOak, Generic3AxisPedalTracker, + GamepadTracker, OgloTactileTracker, TensorPushTracker, JointStateTracker, @@ -52,6 +53,7 @@ def __getattr__(name: str): "FrameMetadataTrackerOak", "FullBodyTracker", "Generic3AxisPedalTracker", + "GamepadTracker", "OgloTactileTracker", "TensorPushTracker", "JointStateTracker", diff --git a/src/core/deviceio_trackers/python/tracker_bindings.cpp b/src/core/deviceio_trackers/python/tracker_bindings.cpp index 7d4f422fcc..d55d7ed17c 100644 --- a/src/core/deviceio_trackers/python/tracker_bindings.cpp +++ b/src/core/deviceio_trackers/python/tracker_bindings.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -154,6 +155,16 @@ PYBIND11_MODULE(_deviceio_trackers, m) { return self.get_data(session); }, py::arg("session"), "Get the current foot pedal tracked state (data is None when no data available)"); + py::class_>(m, "GamepadTracker") + .def(py::init(), py::arg("collection_id"), + py::arg("max_flatbuffer_size") = core::GamepadTracker::DEFAULT_MAX_FLATBUFFER_SIZE, + "Construct a GamepadTracker for the given tensor collection ID") + .def( + "get_gamepad_data", + [](const core::GamepadTracker& self, const core::ITrackerSession& session) -> core::GamepadOutputTrackedT + { return self.get_data(session); }, + py::arg("session"), "Get the current gamepad tracked state (data is None when no data available)"); + py::class_>( m, "OgloTactileTracker") .def(py::init(), py::arg("collection_id"), diff --git a/src/core/live_trackers/cpp/CMakeLists.txt b/src/core/live_trackers/cpp/CMakeLists.txt index db0b260783..83b4896767 100644 --- a/src/core/live_trackers/cpp/CMakeLists.txt +++ b/src/core/live_trackers/cpp/CMakeLists.txt @@ -12,6 +12,7 @@ add_library(live_trackers STATIC live_message_channel_tracker_impl.cpp live_full_body_tracker_pico_impl.cpp live_generic_3axis_pedal_tracker_impl.cpp + live_gamepad_tracker_impl.cpp live_oglo_tactile_tracker_impl.cpp live_tensor_push_tracker_impl.cpp live_haptic_command_reader_tracker_impl.cpp @@ -27,6 +28,7 @@ add_library(live_trackers STATIC live_message_channel_tracker_impl.hpp live_full_body_tracker_pico_impl.hpp live_generic_3axis_pedal_tracker_impl.hpp + live_gamepad_tracker_impl.hpp live_oglo_tactile_tracker_impl.hpp live_tensor_push_tracker_impl.hpp live_haptic_command_reader_tracker_impl.hpp diff --git a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp index ce8e557b45..50dc6ebcb0 100644 --- a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp +++ b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp @@ -33,6 +33,8 @@ class FullBodyTracker; class IFullBodyTrackerImpl; class Generic3AxisPedalTracker; class IGeneric3AxisPedalTrackerImpl; +class GamepadTracker; +class IGamepadTrackerImpl; class OgloTactileTracker; class IOgloTactileTrackerImpl; class TensorPushTracker; @@ -91,6 +93,7 @@ class LiveDeviceIOFactory std::unique_ptr create_full_body_tracker_pico_impl(const FullBodyTracker* tracker); std::unique_ptr create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker); + std::unique_ptr create_gamepad_tracker_impl(const GamepadTracker* tracker); std::unique_ptr create_oglo_tactile_tracker_impl(const OgloTactileTracker* tracker); std::unique_ptr create_tensor_push_tracker_impl(const TensorPushTracker* tracker); std::unique_ptr create_haptic_command_reader_tracker_impl( diff --git a/src/core/live_trackers/cpp/live_deviceio_factory.cpp b/src/core/live_trackers/cpp/live_deviceio_factory.cpp index 028934ef1f..7d7b0b8508 100644 --- a/src/core/live_trackers/cpp/live_deviceio_factory.cpp +++ b/src/core/live_trackers/cpp/live_deviceio_factory.cpp @@ -6,6 +6,7 @@ #include "live_controller_tracker_impl.hpp" #include "live_frame_metadata_tracker_oak_impl.hpp" #include "live_full_body_tracker_pico_impl.hpp" +#include "live_gamepad_tracker_impl.hpp" #include "live_generic_3axis_pedal_tracker_impl.hpp" #include "live_hand_tracker_impl.hpp" #include "live_haptic_command_reader_tracker_impl.hpp" @@ -19,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -100,6 +102,12 @@ std::unique_ptr try_create_generic_pedal_impl(LiveDeviceIOFactory& return typed ? factory.create_generic_3axis_pedal_tracker_impl(typed) : nullptr; } +std::unique_ptr try_create_gamepad_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_gamepad_tracker_impl(typed) : nullptr; +} + std::unique_ptr try_create_tensor_push_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) { auto* typed = dynamic_cast(&tracker); @@ -173,6 +181,7 @@ inline const TrackerDispatchEntry k_tracker_dispatch[] = { make_dispatch_entry(&try_create_message_channel_impl), make_dispatch_entry(&try_create_full_body_pico_impl, "body.pico-xr"), make_dispatch_entry(&try_create_generic_pedal_impl), + make_dispatch_entry(&try_create_gamepad_impl), make_dispatch_entry(&try_create_tensor_push_impl), make_dispatch_entry( &try_create_haptic_command_reader_impl), @@ -481,6 +490,16 @@ std::unique_ptr LiveDeviceIOFactory::create_gener return std::make_unique(handles_, tracker, std::move(channels)); } +std::unique_ptr LiveDeviceIOFactory::create_gamepad_tracker_impl(const GamepadTracker* tracker) +{ + std::unique_ptr channels; + if (should_record(tracker)) + { + channels = LiveGamepadTrackerImpl::create_mcap_channels(*writer_, get_name(tracker)); + } + return std::make_unique(handles_, tracker, std::move(channels)); +} + std::unique_ptr LiveDeviceIOFactory::create_oglo_tactile_tracker_impl( const OgloTactileTracker* tracker) { diff --git a/src/core/live_trackers/cpp/live_gamepad_tracker_impl.cpp b/src/core/live_trackers/cpp/live_gamepad_tracker_impl.cpp new file mode 100644 index 0000000000..4a7a96380c --- /dev/null +++ b/src/core/live_trackers/cpp/live_gamepad_tracker_impl.cpp @@ -0,0 +1,64 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "live_gamepad_tracker_impl.hpp" + +#include +#include + +namespace core +{ + +namespace +{ + +SchemaTrackerConfig make_gamepad_tensor_config(const GamepadTracker* tracker) +{ + SchemaTrackerConfig cfg; + cfg.collection_id = tracker->collection_id(); + cfg.max_flatbuffer_size = tracker->max_flatbuffer_size(); + cfg.tensor_identifier = "gamepad"; + cfg.localized_name = "GamepadTracker"; + return cfg; +} + +} // namespace + +// ============================================================================ +// LiveGamepadTrackerImpl +// ============================================================================ + +std::unique_ptr LiveGamepadTrackerImpl::create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name) +{ + return std::make_unique( + writer, base_name, GamepadRecordingTraits::schema_name, + std::vector( + GamepadRecordingTraits::recording_channels.begin(), GamepadRecordingTraits::recording_channels.end())); +} + +LiveGamepadTrackerImpl::LiveGamepadTrackerImpl(const OpenXRSessionHandles& handles, + const GamepadTracker* tracker, + std::unique_ptr mcap_channels) + : mcap_channels_(std::move(mcap_channels)), + m_schema_reader(handles, + make_gamepad_tensor_config(tracker), + mcap_channels_.get(), + /*mcap_channel_index=*/0, + /*mcap_channel_tracked_index=*/1) +{ +} + +void LiveGamepadTrackerImpl::update(int64_t /*monotonic_time_ns*/) +{ + // Policy: SchemaTracker throws on critical OpenXR/tensor API failures. + // Missing collection/no new data are treated as common non-fatal cases. + m_schema_reader.update(m_tracked.data); +} + +const GamepadOutputTrackedT& LiveGamepadTrackerImpl::get_data() const +{ + return m_tracked; +} + +} // namespace core diff --git a/src/core/live_trackers/cpp/live_gamepad_tracker_impl.hpp b/src/core/live_trackers/cpp/live_gamepad_tracker_impl.hpp new file mode 100644 index 0000000000..59e28a5ba6 --- /dev/null +++ b/src/core/live_trackers/cpp/live_gamepad_tracker_impl.hpp @@ -0,0 +1,52 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "inc/live_trackers/schema_tracker.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace core +{ + +using GamepadMcapChannels = McapTrackerChannels; +using GamepadSchemaTracker = SchemaTracker; + +class LiveGamepadTrackerImpl : public IGamepadTrackerImpl +{ +public: + static std::vector required_extensions() + { + return SchemaTrackerBase::get_required_extensions(); + } + static std::unique_ptr create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name); + + LiveGamepadTrackerImpl(const OpenXRSessionHandles& handles, + const GamepadTracker* tracker, + std::unique_ptr mcap_channels); + + LiveGamepadTrackerImpl(const LiveGamepadTrackerImpl&) = delete; + LiveGamepadTrackerImpl& operator=(const LiveGamepadTrackerImpl&) = delete; + LiveGamepadTrackerImpl(LiveGamepadTrackerImpl&&) = delete; + LiveGamepadTrackerImpl& operator=(LiveGamepadTrackerImpl&&) = delete; + + void update(int64_t monotonic_time_ns) override; + const GamepadOutputTrackedT& get_data() const override; + +private: + std::unique_ptr mcap_channels_; + GamepadSchemaTracker m_schema_reader; + GamepadOutputTrackedT m_tracked; +}; + +} // namespace core diff --git a/src/core/mcap/cpp/inc/mcap/recording_traits.hpp b/src/core/mcap/cpp/inc/mcap/recording_traits.hpp index 4e5b61b4a3..309d5ad228 100644 --- a/src/core/mcap/cpp/inc/mcap/recording_traits.hpp +++ b/src/core/mcap/cpp/inc/mcap/recording_traits.hpp @@ -58,6 +58,13 @@ struct PedalRecordingTraits static constexpr std::array replay_channels = { "pedals_tracked" }; }; +struct GamepadRecordingTraits +{ + static constexpr std::string_view schema_name = "core.GamepadOutputRecord"; + static constexpr std::array recording_channels = { "gamepad", "gamepad_tracked" }; + static constexpr std::array replay_channels = { "gamepad_tracked" }; +}; + struct OgloRecordingTraits { static constexpr std::string_view schema_name = "core.OgloGloveSampleRecord"; diff --git a/src/core/python/deviceio_init.py b/src/core/python/deviceio_init.py index 0f9cc8c71c..ceb2ae2fb4 100644 --- a/src/core/python/deviceio_init.py +++ b/src/core/python/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/core/python/isaacteleop_plugins_gamepad_init.py b/src/core/python/isaacteleop_plugins_gamepad_init.py new file mode 100644 index 0000000000..7a2746c969 --- /dev/null +++ b/src/core/python/isaacteleop_plugins_gamepad_init.py @@ -0,0 +1,4 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Bundled gamepad plugin binary and metadata (``gamepad_plugin``, ``plugin.yaml``).""" diff --git a/src/core/python/isaacteleop_plugins_init.py b/src/core/python/isaacteleop_plugins_init.py new file mode 100644 index 0000000000..afd7b144f6 --- /dev/null +++ b/src/core/python/isaacteleop_plugins_init.py @@ -0,0 +1,19 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Bundled Isaac Teleop plugin binaries. + +Packaged inside the wheel so ``pip install isaacteleop`` alone is sufficient to run +them via :class:`~isaacteleop.teleop_session_manager.PluginManager` -- no separate +``cmake --install`` step required. +""" + +from pathlib import Path + + +def plugin_search_path() -> Path: + """Directory containing every bundled plugin's subdirectory (e.g. ``gamepad/``). + + Pass this to :class:`~isaacteleop.teleop_session_manager.PluginConfig.search_paths`. + """ + return Path(__file__).resolve().parent diff --git a/src/core/python/pyproject.toml.in b/src/core/python/pyproject.toml.in index 793a293668..494900835b 100644 --- a/src/core/python/pyproject.toml.in +++ b/src/core/python/pyproject.toml.in @@ -57,6 +57,8 @@ packages = [ "isaacteleop.cloudxr", "isaacteleop.rig", "isaacteleop.haptic_devices", + "isaacteleop.plugins", + "isaacteleop.plugins.gamepad", @CLOUDXR_EXP_PACKAGES_BLOCK@ @VIZ_PACKAGES_BLOCK@ @ROBOTIC_GROUNDING_PACKAGES_BLOCK@ @@ -74,6 +76,7 @@ isaacteleop = ["*.so", "*.pyd", "*.pyi", "py.typed"] "isaacteleop.schema" = ["*.so", "*.pyd", "*.pyi"] "isaacteleop.cloudxr" = ["native/*.so", "native/*.so.*", "native/openxr_cloudxr.json"] "isaacteleop.haptic_devices" = ["*.so", "*.pyd", "*.pyi"] +"isaacteleop.plugins.gamepad" = ["gamepad_plugin", "plugin.yaml"] @CLOUDXR_EXP_PACKAGE_DATA_BLOCK@ @VIZ_PACKAGE_DATA_BLOCK@ @ROBOTIC_GROUNDING_PACKAGE_DATA_BLOCK@ diff --git a/src/core/replay_trackers/cpp/CMakeLists.txt b/src/core/replay_trackers/cpp/CMakeLists.txt index f87dc13d8f..3ff402004f 100644 --- a/src/core/replay_trackers/cpp/CMakeLists.txt +++ b/src/core/replay_trackers/cpp/CMakeLists.txt @@ -10,6 +10,7 @@ add_library(replay_trackers STATIC replay_controller_tracker_impl.cpp replay_full_body_tracker_impl.cpp replay_generic_3axis_pedal_tracker_impl.cpp + replay_gamepad_tracker_impl.cpp replay_oglo_tactile_tracker_impl.cpp replay_joint_state_tracker_impl.cpp replay_se3_tracker_impl.cpp @@ -22,6 +23,7 @@ add_library(replay_trackers STATIC replay_controller_tracker_impl.hpp replay_full_body_tracker_impl.hpp replay_generic_3axis_pedal_tracker_impl.hpp + replay_gamepad_tracker_impl.hpp replay_oglo_tactile_tracker_impl.hpp replay_joint_state_tracker_impl.hpp replay_se3_tracker_impl.hpp diff --git a/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp b/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp index e231f0c8a5..1d703b5a81 100644 --- a/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp +++ b/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp @@ -21,6 +21,8 @@ class FullBodyTracker; class IFullBodyTrackerImpl; class Generic3AxisPedalTracker; class IGeneric3AxisPedalTrackerImpl; +class GamepadTracker; +class IGamepadTrackerImpl; class OgloTactileTracker; class IOgloTactileTrackerImpl; class TensorPushTracker; @@ -60,6 +62,7 @@ class ReplayDeviceIOFactory std::unique_ptr create_full_body_tracker_impl(const FullBodyTracker* tracker); std::unique_ptr create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker); + std::unique_ptr create_gamepad_tracker_impl(const GamepadTracker* tracker); std::unique_ptr create_oglo_tactile_tracker_impl(const OgloTactileTracker* tracker); std::unique_ptr create_tensor_push_tracker_impl(const TensorPushTracker* tracker); std::unique_ptr create_haptic_command_reader_tracker_impl( diff --git a/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp b/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp index 7a69811261..9395a79c25 100644 --- a/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp +++ b/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp @@ -5,6 +5,7 @@ #include "replay_controller_tracker_impl.hpp" #include "replay_full_body_tracker_impl.hpp" +#include "replay_gamepad_tracker_impl.hpp" #include "replay_generic_3axis_pedal_tracker_impl.hpp" #include "replay_hand_tracker_impl.hpp" #include "replay_haptic_command_reader_tracker_impl.hpp" @@ -17,6 +18,7 @@ #include #include +#include #include #include #include @@ -81,6 +83,12 @@ std::unique_ptr try_create_generic_pedal_impl(ReplayDeviceIOFactor return typed ? factory.create_generic_3axis_pedal_tracker_impl(typed) : nullptr; } +std::unique_ptr try_create_gamepad_impl(ReplayDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_gamepad_tracker_impl(typed) : nullptr; +} + std::unique_ptr try_create_oglo_impl(ReplayDeviceIOFactory& factory, const ITracker& tracker) { auto* typed = dynamic_cast(&tracker); @@ -126,6 +134,7 @@ inline const TryCreateFn k_tracker_dispatch[] = { &try_create_controller_impl, &try_create_full_body_impl, &try_create_generic_pedal_impl, + &try_create_gamepad_impl, &try_create_oglo_impl, &try_create_tensor_push_impl, &try_create_haptic_command_reader_impl, @@ -197,6 +206,11 @@ std::unique_ptr ReplayDeviceIOFactory::create_gen return std::make_unique(open_reader(filename_), get_name(tracker)); } +std::unique_ptr ReplayDeviceIOFactory::create_gamepad_tracker_impl(const GamepadTracker* tracker) +{ + return std::make_unique(open_reader(filename_), get_name(tracker)); +} + std::unique_ptr ReplayDeviceIOFactory::create_oglo_tactile_tracker_impl( const OgloTactileTracker* tracker) { diff --git a/src/core/replay_trackers/cpp/replay_gamepad_tracker_impl.cpp b/src/core/replay_trackers/cpp/replay_gamepad_tracker_impl.cpp new file mode 100644 index 0000000000..3698442bf6 --- /dev/null +++ b/src/core/replay_trackers/cpp/replay_gamepad_tracker_impl.cpp @@ -0,0 +1,49 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "replay_gamepad_tracker_impl.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace core +{ + +// ============================================================================ +// ReplayGamepadTrackerImpl +// ============================================================================ + +ReplayGamepadTrackerImpl::ReplayGamepadTrackerImpl(std::unique_ptr reader, std::string_view base_name) + : mcap_viewers_( + std::make_unique(std::move(reader), + base_name, + std::vector(GamepadRecordingTraits::replay_channels.begin(), + GamepadRecordingTraits::replay_channels.end()))) +{ +} + +const GamepadOutputTrackedT& ReplayGamepadTrackerImpl::get_data() const +{ + return tracked_; +} + +void ReplayGamepadTrackerImpl::update(int64_t /*monotonic_time_ns*/) +{ + auto record = mcap_viewers_->read(0); + if (record) + { + tracked_.data = std::move(record->data); + } + else + { + std::cerr << "ReplayGamepadTrackerImpl: gamepad data not found" << std::endl; + tracked_.data.reset(); + } +} + +} // namespace core diff --git a/src/core/replay_trackers/cpp/replay_gamepad_tracker_impl.hpp b/src/core/replay_trackers/cpp/replay_gamepad_tracker_impl.hpp new file mode 100644 index 0000000000..6661ce43aa --- /dev/null +++ b/src/core/replay_trackers/cpp/replay_gamepad_tracker_impl.hpp @@ -0,0 +1,37 @@ +// 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 +{ + +using GamepadMcapViewers = McapTrackerViewers; + +class ReplayGamepadTrackerImpl : public IGamepadTrackerImpl +{ +public: + ReplayGamepadTrackerImpl(std::unique_ptr reader, std::string_view base_name); + + ReplayGamepadTrackerImpl(const ReplayGamepadTrackerImpl&) = delete; + ReplayGamepadTrackerImpl& operator=(const ReplayGamepadTrackerImpl&) = delete; + ReplayGamepadTrackerImpl(ReplayGamepadTrackerImpl&&) = delete; + ReplayGamepadTrackerImpl& operator=(ReplayGamepadTrackerImpl&&) = delete; + + void update(int64_t monotonic_time_ns) override; + const GamepadOutputTrackedT& get_data() const override; + +private: + GamepadOutputTrackedT tracked_; + std::unique_ptr mcap_viewers_; +}; + +} // namespace core diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py b/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py index b1b583e572..e1e4496d80 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py +++ b/src/core/retargeting_engine/python/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/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py b/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py index cb86f3bed8..8ee0107253 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/deviceio_tensor_types.py @@ -21,6 +21,7 @@ Generic3AxisPedalOutputTrackedT, JointStateOutputTrackedT, FullBodyPoseTrackedT, + GamepadOutputTrackedT, MessageChannelMessagesTrackedT, ) @@ -101,6 +102,26 @@ def validate_value(self, value: Any) -> None: ) +class GamepadOutputTrackedType(TensorType): + """GamepadOutputTrackedT wrapper type from DeviceIO GamepadTracker.""" + + def __init__(self, name: str) -> None: + super().__init__(name) + + def _check_instance_compatibility(self, other: TensorType) -> bool: + if not isinstance(other, GamepadOutputTrackedType): + raise TypeError( + f"Expected GamepadOutputTrackedType, got {type(other).__name__}" + ) + return True + + def validate_value(self, value: Any) -> None: + if not isinstance(value, GamepadOutputTrackedT): + raise TypeError( + f"Expected GamepadOutputTrackedT for '{self.name}', got {type(value).__name__}" + ) + + class JointStateOutputTrackedType(TensorType): """JointStateOutputTrackedT wrapper type from DeviceIO JointStateTracker.""" @@ -237,6 +258,18 @@ def DeviceIOGeneric3AxisPedalOutputTracked() -> TensorGroupType: ) +def DeviceIOGamepadOutputTracked() -> TensorGroupType: + """Tracked gamepad data from DeviceIO GamepadTracker. + + Contains: + gamepad_tracked: GamepadOutputTrackedT wrapper (always set; .data is 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/core/retargeting_engine/python/deviceio_source_nodes/gamepad_source.py b/src/core/retargeting_engine/python/deviceio_source_nodes/gamepad_source.py new file mode 100644 index 0000000000..db3f452f4a --- /dev/null +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/gamepad_source.py @@ -0,0 +1,183 @@ +# 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, + GamepadOutputTrackedT, + ) + +# 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 GamepadOutputTrackedT. + """ + tracked = self._gamepad_tracker.get_gamepad_data(deviceio_session) + tg = TensorGroup(DeviceIOGamepadOutputTracked()) + tg[0] = tracked + 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 GamepadOutputTrackedT 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 GamepadOutputTrackedT wrapper + outputs: Dict with "gamepad_buttons" and "gamepad_axes" OptionalTensorGroups + context: Shared ComputeContext for the current step (carries GraphTime). + """ + import numpy as np + + tracked: GamepadOutputTrackedT = inputs["deviceio_gamepad"][0] + state: GamepadOutput | None = tracked.data + + 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/core/retargeting_engine_tests/python/test_gamepad.py b/src/core/retargeting_engine_tests/python/test_gamepad.py new file mode 100644 index 0000000000..6abb27a1be --- /dev/null +++ b/src/core/retargeting_engine_tests/python/test_gamepad.py @@ -0,0 +1,290 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +End-to-end tests for the gamepad device: raw joystick-API button/axis state +(constructed via the real schema Python bindings, no live device/plugin/OpenXR session +involved) flowing through GamepadSource into GamepadToSe3RelRetargeter, +GamepadGripperRetargeter, and GamepadToSe2Retargeter. + +These exercise the full feature as a whole -- schema -> source -> retargeters -- so a +regression anywhere in that chain (a field rename, an index drift, a sign flip) fails +here, rather than testing each stage's internals in isolation. +""" + +import numpy as np +import pytest +from isaacteleop.retargeters import ( + GamepadGripperRetargeter, + GamepadToSe2Retargeter, + GamepadToSe2RetargeterConfig, + GamepadToSe3RelRetargeter, + GamepadToSe3RelRetargeterConfig, +) +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.schema import GamepadOutput, GamepadOutputTrackedT + +# 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().""" + if pressed_buttons is None: + tracked = GamepadOutputTrackedT() # data is None -> inactive + else: + tracked = GamepadOutputTrackedT( + GamepadOutput(pressed_buttons, axes or [0.0] * 8, True) + ) + + input_spec = src.input_spec() + tg = TensorGroup(input_spec["deviceio_gamepad"]) + tg[0] = tracked + + outputs = {name: _make_output_group(gt) for name, gt in src.output_spec().items()} + src.compute({"deviceio_gamepad": tg}, outputs) + return outputs + + +class TestGamepadEndToEnd: + 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_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_se3_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) + + 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_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_inactive_device_yields_safe_defaults(self): + """No sample yet (tracker/plugin not streaming) -> zero delta, no gripper state change.""" + src = _gamepad_source() + src_outputs = _run_source(src, None) + assert src_outputs["gamepad_axes"].is_none + assert src_outputs["gamepad_buttons"].is_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) + + 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_buttons_bitmap_covers_indices_outside_gripper_button(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 + + bitmap = np.asarray(src_outputs["gamepad_buttons"][0]) + assert bitmap[5] == 1 + assert bitmap.sum() == 1 + + 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 + + def test_se2_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_se2_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_se2_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/src/core/schema/fbs/gamepad.fbs b/src/core/schema/fbs/gamepad.fbs new file mode 100644 index 0000000000..375ef4f3dd --- /dev/null +++ b/src/core/schema/fbs/gamepad.fbs @@ -0,0 +1,38 @@ +// 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 when the parent Tracked/Record wrapper's data is non-null. +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); +} + +// Tracked wrapper for the in-memory tracker API (data is null when no sample is available). +table GamepadOutputTracked { + data: GamepadOutput (id: 0); +} + +// 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 6756bbd6ab..0907150b47 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..171a27477b --- /dev/null +++ b/src/core/schema/python/gamepad_bindings.h @@ -0,0 +1,101 @@ +// 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). + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_gamepad(py::module& m) +{ + py::class_>(m, "GamepadOutput") + .def(py::init([]() { return std::make_shared(); })) + .def(py::init( + [](std::vector pressed_buttons, std::vector axes, bool is_valid) + { + auto obj = std::make_shared(); + obj->pressed_buttons = std::move(pressed_buttons); + obj->axes = std::move(axes); + obj->is_valid = is_valid; + return obj; + }), + py::arg("pressed_buttons"), py::arg("axes"), py::arg("is_valid")) + .def_property( + "pressed_buttons", [](const GamepadOutputT& self) { return self.pressed_buttons; }, + [](GamepadOutputT& self, std::vector val) { self.pressed_buttons = std::move(val); }) + .def_property( + "axes", [](const GamepadOutputT& self) { return self.axes; }, + [](GamepadOutputT& self, std::vector val) { self.axes = std::move(val); }) + .def_property( + "is_valid", [](const GamepadOutputT& self) { return self.is_valid; }, + [](GamepadOutputT& self, bool val) { self.is_valid = val; }) + .def("__repr__", + [](const GamepadOutputT& self) + { + std::string result = "GamepadOutput(pressed_buttons=["; + for (size_t i = 0; i < self.pressed_buttons.size(); ++i) + { + if (i > 0) + result += ", "; + result += std::to_string(self.pressed_buttons[i]); + } + result += "], axes=["; + for (size_t i = 0; i < self.axes.size(); ++i) + { + if (i > 0) + result += ", "; + result += std::to_string(self.axes[i]); + } + result += "], is_valid=" + std::to_string(self.is_valid) + ")"; + return result; + }); + + py::class_>(m, "GamepadOutputRecord") + .def(py::init<>()) + .def(py::init( + [](const GamepadOutputT& data, const DeviceDataTimestamp& timestamp) + { + auto obj = std::make_shared(); + obj->data = std::make_shared(data); + obj->timestamp = std::make_shared(timestamp); + return obj; + }), + py::arg("data"), py::arg("timestamp")) + .def_property_readonly( + "data", [](const GamepadOutputRecordT& self) -> std::shared_ptr { return self.data; }) + .def_readonly("timestamp", &GamepadOutputRecordT::timestamp) + .def("__repr__", [](const GamepadOutputRecordT& self) + { return "GamepadOutputRecord(data=" + std::string(self.data ? "GamepadOutput(...)" : "None") + ")"; }); + + py::class_>(m, "GamepadOutputTrackedT") + .def(py::init<>()) + .def(py::init( + [](const GamepadOutputT& data) + { + auto obj = std::make_shared(); + obj->data = std::make_shared(data); + return obj; + }), + py::arg("data")) + .def_property_readonly( + "data", [](const GamepadOutputTrackedT& self) -> std::shared_ptr { return self.data; }) + .def("__repr__", [](const GamepadOutputTrackedT& self) + { return std::string("GamepadOutputTrackedT(data=") + (self.data ? "GamepadOutput(...)" : "None") + ")"; }); +} + +} // namespace core diff --git a/src/core/schema/python/schema_init.py b/src/core/schema/python/schema_init.py index cc1d3265c2..86b67a0827 100644 --- a/src/core/schema/python/schema_init.py +++ b/src/core/schema/python/schema_init.py @@ -46,6 +46,10 @@ JointStateOutput, JointStateOutputTrackedT, JointStateOutputRecord, + # Gamepad types (raw joystick-API button/axis state). + GamepadOutput, + GamepadOutputTrackedT, + GamepadOutputRecord, # SE3 tracker types (generic 6-DoF pose sources: tracker pucks, mocap rigid bodies, ...). # Record classes drop the T suffix in Python by family convention. Se3TrackerPoseT, @@ -132,6 +136,10 @@ def __getattr__(name: str): "JointStateOutput", "JointStateOutputTrackedT", "JointStateOutputRecord", + # Gamepad types (raw joystick-API button/axis state). + "GamepadOutput", + "GamepadOutputTrackedT", + "GamepadOutputRecord", # SE3 tracker types (generic 6-DoF pose sources). "Se3TrackerPoseT", "Se3TrackerPoseTrackedT", diff --git a/src/core/schema/python/schema_module.cpp b/src/core/schema/python/schema_module.cpp index cbd2ca4915..dd69809871 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" @@ -53,6 +54,9 @@ PYBIND11_MODULE(_schema, m) // Bind SE3 tracker types (Se3TrackerPoseT table) for generic 6-DoF pose sources. core::bind_se3_tracker(m); + // Bind gamepad types (GamepadOutputT table) for raw joystick-API button/axis state. + core::bind_gamepad(m); + // Bind message channel types (MessageChannelMessages table). core::bind_message_channel(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..378072a73b --- /dev/null +++ b/src/plugins/gamepad/gamepad_plugin.cpp @@ -0,0 +1,176 @@ +// 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 must not keep publishing the last-known + // stick/trigger deflection as valid motion. + 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/retargeters/__init__.py b/src/retargeters/__init__.py index ae3a9de19f..fbadd86e6b 100644 --- a/src/retargeters/__init__.py +++ b/src/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 - SO101GripperRetargeter: Proportional (analog) jaw closedness for the SO-101 gripper - JointStateRetargeter: Generic joint-space device (leader arm, exoskeleton) -> joint or EE action @@ -101,6 +104,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: full-pose clutch EE pose, analog gripper) "SO101ClutchRetargeter": ( ".SO101.clutch_retargeter", @@ -214,6 +244,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/retargeters/gamepad_se2_retargeter.py b/src/retargeters/gamepad_se2_retargeter.py new file mode 100644 index 0000000000..e576f42459 --- /dev/null +++ b/src/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/retargeters/gamepad_se3_retargeter.py b/src/retargeters/gamepad_se3_retargeter.py new file mode 100644 index 0000000000..c89f0a6f62 --- /dev/null +++ b/src/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