From b699afd90c25e2b0bcee7247ad4cdafad93a863b Mon Sep 17 00:00:00 2001 From: Rafael Wiltz Date: Tue, 25 Aug 2026 10:19:02 -0400 Subject: [PATCH 1/8] Add spacemouse SE3/SE2 device: schema, tracker, plugin, retargeters Signed-off-by: Rafael Wiltz --- CMakeLists.txt | 1 + src/core/deviceio_trackers/trackers.toml | 7 + src/core/schema/fbs/spacemouse.fbs | 46 ++++ src/core/schema/python/CMakeLists.txt | 1 + src/core/schema/python/schema_module.cpp | 4 + src/core/schema/python/spacemouse_bindings.h | 64 ++++++ src/plugins/spacemouse/CMakeLists.txt | 23 ++ src/plugins/spacemouse/README.md | 54 +++++ src/plugins/spacemouse/main.cpp | 191 +++++++++++++++++ src/plugins/spacemouse/plugin.yaml | 11 + src/plugins/spacemouse/spacemouse_plugin.cpp | 197 +++++++++++++++++ src/plugins/spacemouse/spacemouse_plugin.hpp | 66 ++++++ src/python/isaacteleop/deviceio/__init__.py | 4 + .../isaacteleop/retargeters/__init__.py | 35 +++ .../retargeters/spacemouse_se2_retargeter.py | 92 ++++++++ .../retargeters/spacemouse_se3_retargeter.py | 153 +++++++++++++ .../deviceio_source_nodes/__init__.py | 14 ++ .../deviceio_tensor_types.py | 19 ++ .../spacemouse_source.py | 202 ++++++++++++++++++ src/python/isaacteleop/schema/__init__.py | 6 + .../test_spacemouse_retargeter.py | 188 ++++++++++++++++ .../test_spacemouse_source.py | 86 ++++++++ tests/python/core/schema/test_spacemouse.py | 123 +++++++++++ 23 files changed, 1587 insertions(+) create mode 100644 src/core/schema/fbs/spacemouse.fbs create mode 100644 src/core/schema/python/spacemouse_bindings.h create mode 100644 src/plugins/spacemouse/CMakeLists.txt create mode 100644 src/plugins/spacemouse/README.md create mode 100644 src/plugins/spacemouse/main.cpp create mode 100644 src/plugins/spacemouse/plugin.yaml create mode 100644 src/plugins/spacemouse/spacemouse_plugin.cpp create mode 100644 src/plugins/spacemouse/spacemouse_plugin.hpp create mode 100644 src/python/isaacteleop/retargeters/spacemouse_se2_retargeter.py create mode 100644 src/python/isaacteleop/retargeters/spacemouse_se3_retargeter.py create mode 100644 src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/spacemouse_source.py create mode 100644 tests/python/core/retargeting_engine/test_spacemouse_retargeter.py create mode 100644 tests/python/core/retargeting_engine/test_spacemouse_source.py create mode 100644 tests/python/core/schema/test_spacemouse.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 998e78b39c..258eb96ef4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -189,6 +189,7 @@ if(BUILD_PLUGINS) add_subdirectory(src/plugins/rebot_devarm_leader) add_subdirectory(src/plugins/manus) add_subdirectory(src/plugins/haptikos) + add_subdirectory(src/plugins/spacemouse) if(BUILD_PLUGIN_NOITOM_MOCAP) add_subdirectory(src/plugins/noitom_mocap) endif() diff --git a/src/core/deviceio_trackers/trackers.toml b/src/core/deviceio_trackers/trackers.toml index 813ec47581..5a2809a0d1 100644 --- a/src/core/deviceio_trackers/trackers.toml +++ b/src/core/deviceio_trackers/trackers.toml @@ -28,6 +28,13 @@ traits = "PedalRecordingTraits" max_flatbuffer_size = 256 python_accessor = "get_pedal_data" +[[tracker]] +name = "spacemouse" +table = "SpaceMouseOutput" +class = "SpaceMouseTracker" +max_flatbuffer_size = 512 +python_accessor = "get_spacemouse_data" + [[tracker]] name = "haptic_command" direction = "push" diff --git a/src/core/schema/fbs/spacemouse.fbs b/src/core/schema/fbs/spacemouse.fbs new file mode 100644 index 0000000000..70e86217ce --- /dev/null +++ b/src/core/schema/fbs/spacemouse.fbs @@ -0,0 +1,46 @@ +// 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 3Dconnexion SpaceMouse-family HID device: the current translation and +// rotation axis readings and the set of currently-held button indices. Carries no +// semantic mapping -- which axis/button means what (a position delta, a rotation delta, +// a toggle) is entirely up to the consuming retargeter. +// +// Validated devices: +// - SpaceMouse Compact, SpaceMouse Wireless, SpaceNavigator for Notebooks, +// 3Dconnexion Universal Receiver. +// +// All fields are always present whenever this table itself is present. +table SpaceMouseOutput { + // Translation axis readings [x, y, z], normalized to [-1, 1]. Index i is the + // value of translation axis i, in the order reported by the device's HID report + // (see spacemouse_plugin.cpp). + translation: [float] (id: 0); + + // Rotation axis readings [x, y, z], normalized to [-1, 1], same convention as + // translation. + rotation: [float] (id: 1); + + // Button indices currently held down (bit position in the device's button + // report byte), in no particular order. + pressed_buttons: [ushort] (id: 2); + + // Whether the tracker has emitted at least one sample. + is_valid: bool (id: 3); +} + +// MCAP recording wrapper for SpaceMouseOutput. +// +// Record types are the root types written to MCAP channels by the McapRecorder. +// Trackers serialize into Record types via their serialize() method, but the +// public query API returns the inner data type directly. +table SpaceMouseOutputRecord { + data: SpaceMouseOutput (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type SpaceMouseOutputRecord; diff --git a/src/core/schema/python/CMakeLists.txt b/src/core/schema/python/CMakeLists.txt index 5f01ee3110..bd73f46504 100644 --- a/src/core/schema/python/CMakeLists.txt +++ b/src/core/schema/python/CMakeLists.txt @@ -16,6 +16,7 @@ pybind11_add_module(schema_py schema_array_views.h schema_serialized.h se3_tracker_bindings.h + spacemouse_bindings.h schema_module.cpp ) diff --git a/src/core/schema/python/schema_module.cpp b/src/core/schema/python/schema_module.cpp index 420d2b6965..f1e89933da 100644 --- a/src/core/schema/python/schema_module.cpp +++ b/src/core/schema/python/schema_module.cpp @@ -18,6 +18,7 @@ #include "pedals_bindings.h" #include "pose_bindings.h" #include "se3_tracker_bindings.h" +#include "spacemouse_bindings.h" #include "timestamp_bindings.h" namespace py = pybind11; @@ -53,6 +54,9 @@ PYBIND11_MODULE(_schema, m) // Bind SE3 tracker types (Se3TrackerPose table) for generic 6-DoF pose sources. core::bind_se3_tracker(m); + // Bind SpaceMouse types (SpaceMouseOutput table) for raw 3Dconnexion axis/button state. + core::bind_spacemouse(m); + // Bind message channel types (MessageChannelMessages table). core::bind_message_channel(m); diff --git a/src/core/schema/python/spacemouse_bindings.h b/src/core/schema/python/spacemouse_bindings.h new file mode 100644 index 0000000000..cef3c64b02 --- /dev/null +++ b/src/core/schema/python/spacemouse_bindings.h @@ -0,0 +1,64 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Python bindings for the SpaceMouse FlatBuffer schema. +// Types: SpaceMouseOutput (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_spacemouse(py::module& m) +{ + serialized_class(m, "SpaceMouseOutput", "Encoded raw SpaceMouse axis/button state.") + .def(py::init( + [](std::vector translation, std::vector rotation, std::vector pressed_buttons, + bool is_valid) + { + SpaceMouseOutputT native; + native.translation = std::move(translation); + native.rotation = std::move(rotation); + native.pressed_buttons = std::move(pressed_buttons); + native.is_valid = is_valid; + return pack(native); + }), + py::arg("translation"), py::arg("rotation"), py::arg("pressed_buttons"), py::arg("is_valid"), + "Encode a SpaceMouse axis/button snapshot.") + .def_property_readonly("translation", vector_field(&SpaceMouseOutput::translation)) + .def_property_readonly("rotation", vector_field(&SpaceMouseOutput::rotation)) + .def_property_readonly("pressed_buttons", vector_field(&SpaceMouseOutput::pressed_buttons)) + .def_property_readonly("is_valid", field(&SpaceMouseOutput::is_valid)) + .def("__repr__", + [](const Serialized& self) + { + std::string result = "SpaceMouseOutput(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 += "], is_valid=" + std::to_string(self->is_valid()) + ")"; + return result; + }); + + bind_record(m, "SpaceMouseOutputRecord", "SpaceMouseOutput"); +} + +} // namespace core diff --git a/src/plugins/spacemouse/CMakeLists.txt b/src/plugins/spacemouse/CMakeLists.txt new file mode 100644 index 0000000000..aebfa30bbf --- /dev/null +++ b/src/plugins/spacemouse/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 spacemouse plugin (Linux only)") + add_custom_target(spacemouse_plugin + COMMAND ${CMAKE_COMMAND} -E echo "Skipping spacemouse: Linux only") + return() +endif() + +add_executable(spacemouse_plugin + main.cpp + spacemouse_plugin.cpp +) + +target_link_libraries(spacemouse_plugin PRIVATE + pusherio::pusherio + oxr::oxr_core + isaacteleop_schema +) + +install(TARGETS spacemouse_plugin RUNTIME DESTINATION plugins/spacemouse) +install(FILES plugin.yaml README.md DESTINATION plugins/spacemouse) diff --git a/src/plugins/spacemouse/README.md b/src/plugins/spacemouse/README.md new file mode 100644 index 0000000000..127d4d6ced --- /dev/null +++ b/src/plugins/spacemouse/README.md @@ -0,0 +1,54 @@ + + +# SpaceMouse Plugin + +Reads a 3Dconnexion SpaceMouse-family device from `/dev/hidraw*` and pushes `SpaceMouseOutput` +via OpenXR. Use with `SpaceMouseTracker` with the same `collection_id`. + +Reports raw axis/button state only, with no semantic mapping to position, rotation, or +commands -- that mapping belongs in a retargeter (e.g. `SpaceMouseToSe3RelRetargeter`) +consuming this tracker's output. + +Self-discovers its device (the first hidraw device under `/sys/class/hidraw/` whose +`HID_NAME` matches a validated product name), 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 +./spacemouse_plugin --plugin-root-id=spacemouse +``` + +Manual / standalone, with an explicit device: + +```bash +./spacemouse_plugin [device_path] [--combined-report] [--plugin-root-id=] +``` + +- **device_path**: Optional. Defaults to the first matching hidraw device under + `/sys/class/hidraw/`. Identify a specific device with + `cat /sys/class/hidraw/hidraw*/device/uevent` (look for a `HID_NAME=` line) or `lsusb`. + Reading `/dev/hidraw*` typically requires membership in the `input` group (or a udev + rule granting access). +- **--combined-report**: Only needed with an explicit device path for a "3Dconnexion + Universal Receiver" (auto-discovery sets this automatically); packs translation and + rotation into a single 13-byte report instead of two separate 7-byte reports. +- **collection_id**: Default `spacemouse`. Match this when creating `SpaceMouseTracker`. + +## Validated devices + +- SpaceMouse Compact +- SpaceMouse Wireless +- SpaceNavigator for Notebooks +- 3Dconnexion Universal Receiver + +## Axis/button mapping + +Reports the current translation (`[x, y, z]`) and rotation (`[x, y, z]`) axis readings, +normalized to `[-1, 1]`, and the set of currently-held button indices (bit position in the +device's button report byte). Linux only. diff --git a/src/plugins/spacemouse/main.cpp b/src/plugins/spacemouse/main.cpp new file mode 100644 index 0000000000..0d130b054f --- /dev/null +++ b/src/plugins/spacemouse/main.cpp @@ -0,0 +1,191 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "spacemouse_plugin.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::spacemouse; + +namespace +{ + +// Product strings validated by Isaac Lab's Se3SpaceMouse / Se2SpaceMouse. The "Universal +// Receiver" reports translation and rotation combined in a single 13-byte report; the +// others report them as two separate 7-byte reports. +struct KnownDevice +{ + std::string_view product_name; + bool combined_report; +}; + +constexpr KnownDevice kKnownDevices[] = { + { "SpaceMouse Compact", false }, + { "SpaceMouse Wireless", false }, + { "SpaceNavigator for Notebooks", false }, + { "3Dconnexion Universal Receiver", true }, +}; + +// Reads HID_NAME= out of a hidraw device's sysfs uevent file (e.g. +// /sys/class/hidraw/hidraw0/device/uevent), or nullopt if unavailable. +std::optional read_hid_name(const std::filesystem::path& uevent_path) +{ + std::ifstream file(uevent_path); + if (!file.is_open()) + return std::nullopt; + + std::string line; + while (std::getline(file, line)) + { + constexpr std::string_view kPrefix = "HID_NAME="; + if (line.starts_with(kPrefix)) + return line.substr(kPrefix.size()); + } + return std::nullopt; +} + +struct DiscoveredDevice +{ + std::string device_path; + bool combined_report; +}; + +// Scans /sys/class/hidraw/hidraw* for the first device whose HID_NAME matches a known +// SpaceMouse-family product name, and returns the corresponding /dev/hidrawN path. 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_spacemouse_device() +{ + const std::filesystem::path hidraw_class_dir = "/sys/class/hidraw"; + std::error_code ec; + if (!std::filesystem::exists(hidraw_class_dir, ec)) + return std::nullopt; + + std::vector hidraw_names; + for (const auto& entry : std::filesystem::directory_iterator(hidraw_class_dir, ec)) + hidraw_names.push_back(entry.path().filename().string()); + std::sort(hidraw_names.begin(), hidraw_names.end()); + + for (const auto& hidraw_name : hidraw_names) + { + const auto uevent_path = hidraw_class_dir / hidraw_name / "device" / "uevent"; + const auto hid_name = read_hid_name(uevent_path); + if (!hid_name) + continue; + + for (const auto& known : kKnownDevices) + { + // HID_NAME is typically " "; match by substring so a + // manufacturer prefix (e.g. "3Dconnexion SpaceMouse Compact") still matches. + if (hid_name->find(known.product_name) != std::string::npos) + { + return DiscoveredDevice{ "/dev/" + hidraw_name, known.combined_report }; + } + } + } + return std::nullopt; +} + +// 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; --combined-report opts into the +// Universal Receiver's single-report layout for that override. +struct ParsedArgs +{ + std::optional device_path; + bool combined_report = false; + std::string collection_id = "spacemouse"; +}; + +ParsedArgs parse_args(int argc, char** argv) +{ + ParsedArgs parsed; + constexpr std::string_view kRootIdPrefix = "--plugin-root-id="; + constexpr std::string_view kCombinedReportFlag = "--combined-report"; + 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 == kCombinedReportFlag) + { + parsed.combined_report = true; + } + 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: spacemouse_plugin [device_path] [--combined-report] [--plugin-root-id=]" + << std::endl; + return 1; + } + + const ParsedArgs args = parse_args(argc, argv); + std::optional device_path = args.device_path; + bool combined_report = args.combined_report; + if (!device_path) + { + auto discovered = discover_spacemouse_device(); + if (!discovered) + { + std::cerr << argv[0] + << ": No SpaceMouse-family device found under /sys/class/hidraw/ and none given explicitly." + << std::endl; + return 1; + } + device_path = discovered->device_path; + combined_report = discovered->combined_report; + } + + std::cout << "SpaceMouse (device: " << *device_path << ", collection: " << args.collection_id << ")" << std::endl; + + SpaceMousePlugin plugin(*device_path, args.collection_id, combined_report); + + // 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/spacemouse/plugin.yaml b/src/plugins/spacemouse/plugin.yaml new file mode 100644 index 0000000000..47565d00ad --- /dev/null +++ b/src/plugins/spacemouse/plugin.yaml @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: spacemouse +description: "Raw 3Dconnexion SpaceMouse axis/button state via Linux hidraw device" +command: "./spacemouse_plugin" +version: "1.0.0" +devices: + - path: "/spacemouse" + type: "spacemouse" + description: "SpaceMouse axis/button state from /dev/hidraw*" diff --git a/src/plugins/spacemouse/spacemouse_plugin.cpp b/src/plugins/spacemouse/spacemouse_plugin.cpp new file mode 100644 index 0000000000..fe57a7eaae --- /dev/null +++ b/src/plugins/spacemouse/spacemouse_plugin.cpp @@ -0,0 +1,197 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "spacemouse_plugin.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace spacemouse +{ + +namespace +{ + +// Combined (Universal Receiver) reports are 13 bytes: report ID + 6 translation bytes + +// 6 rotation bytes. Separate reports are 7 bytes: report ID + 6 axis bytes. +constexpr size_t kCombinedReportSize = 13; +constexpr size_t kSeparateReportSize = 7; +constexpr double kAxisScale = 350.0; +constexpr size_t kMaxFlatbufferSize = 512; + +// Two bytes, little-endian, to a signed 16-bit integer -- matches Isaac Lab's +// isaaclab.devices.spacemouse.utils._to_int16. +int16_t to_int16(uint8_t low, uint8_t high) +{ + return static_cast(static_cast(low) | (static_cast(high) << 8)); +} + +// Matches isaaclab.devices.spacemouse.utils.convert_buffer: normalize and clamp to [-1, 1]. +float convert_axis(uint8_t low, uint8_t high) +{ + double value = static_cast(to_int16(low, high)) / kAxisScale; + return static_cast(std::max(-1.0, std::min(1.0, value))); +} + +} // namespace + +SpaceMousePlugin::SpaceMousePlugin(const std::string& device_path, const std::string& collection_id, bool combined_report) + : device_path_(device_path), + combined_report_(combined_report), + session_(std::make_shared("SpaceMousePlugin", core::SchemaPusher::get_required_extensions())), + pusher_(session_->get_handles(), + core::SchemaPusherConfig{ .collection_id = collection_id, + .max_flatbuffer_size = kMaxFlatbufferSize, + .tensor_identifier = "spacemouse", + .localized_name = "SpaceMouse", + .app_name = "SpaceMousePlugin" }) +{ + if (!open_device()) + throw std::runtime_error("SpaceMousePlugin: Failed to open " + device_path + " (" + strerror(errno) + ")"); +} + +SpaceMousePlugin::~SpaceMousePlugin() +{ + close_device(); +} + +void SpaceMousePlugin::update() +{ + if (device_fd_ < 0) + { + open_device(); + if (device_fd_ < 0) + { + push_current_state(); + return; + } + } + + const size_t report_size = combined_report_ ? kCombinedReportSize : kSeparateReportSize; + + 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; + } + + uint8_t buffer[kCombinedReportSize]; + ssize_t n = read(device_fd_, buffer, report_size); + if (n <= 0) + { + if (n < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)) + break; + close_device(); + push_current_state(); + return; + } + + const uint8_t report_id = buffer[0]; + if (report_id == 1 && static_cast(n) >= 7) + { + translation_[0] = convert_axis(buffer[1], buffer[2]); + translation_[1] = convert_axis(buffer[3], buffer[4]); + translation_[2] = convert_axis(buffer[5], buffer[6]); + if (combined_report_ && static_cast(n) >= 13) + { + rotation_[0] = convert_axis(buffer[7], buffer[8]); + rotation_[1] = convert_axis(buffer[9], buffer[10]); + rotation_[2] = convert_axis(buffer[11], buffer[12]); + } + } + else if (report_id == 2 && !combined_report_ && static_cast(n) >= 7) + { + rotation_[0] = convert_axis(buffer[1], buffer[2]); + rotation_[1] = convert_axis(buffer[3], buffer[4]); + rotation_[2] = convert_axis(buffer[5], buffer[6]); + } + else if (report_id == 3 && static_cast(n) >= 2) + { + // Button report: buffer[1] is a bitmask (bit i = button i currently held). + pressed_buttons_.clear(); + for (uint16_t bit = 0; bit < 8; ++bit) + { + if ((buffer[1] & (1u << bit)) != 0u) + pressed_buttons_.insert(bit); + } + } + } + + push_current_state(); +} + +bool SpaceMousePlugin::open_device() +{ + assert(device_fd_ < 0); + + int fd = open(device_path_.c_str(), O_RDONLY | O_NONBLOCK); + if (fd < 0) + return false; + + device_fd_ = fd; + std::cout << "SpaceMousePlugin: Opened " << device_path_ << (combined_report_ ? " (combined report)" : "") + << std::endl; + return true; +} + +void SpaceMousePlugin::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(); +} + +void SpaceMousePlugin::push_current_state() +{ + core::SpaceMouseOutputT out; + out.translation.assign(translation_.begin(), translation_.end()); + out.rotation.assign(rotation_.begin(), rotation_.end()); + out.pressed_buttons.assign(pressed_buttons_.begin(), pressed_buttons_.end()); + out.is_valid = true; + + auto sample_time_ns = core::os_monotonic_now_ns(); + + flatbuffers::FlatBufferBuilder builder(kMaxFlatbufferSize); + auto offset = core::SpaceMouseOutput::Pack(builder, &out); + builder.Finish(offset); + pusher_.push_buffer(builder.GetBufferPointer(), builder.GetSize(), sample_time_ns, sample_time_ns); +} + +} // namespace spacemouse +} // namespace plugins diff --git a/src/plugins/spacemouse/spacemouse_plugin.hpp b/src/plugins/spacemouse/spacemouse_plugin.hpp new file mode 100644 index 0000000000..72720e7f76 --- /dev/null +++ b/src/plugins/spacemouse/spacemouse_plugin.hpp @@ -0,0 +1,66 @@ +// 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 spacemouse +{ + +/*! + * @brief Reads a 3Dconnexion SpaceMouse-family HID device (e.g. /dev/hidraw0), + * tracks the current translation/rotation axis readings and the set of + * currently-held button indices, and pushes SpaceMouseOutput via OpenXR + * SchemaPusher. Carries no semantic mapping -- axes/buttons are reported + * as-is (raw HID report bytes, decoded to normalized [-1, 1] axis values). + */ +class SpaceMousePlugin +{ +public: + // combined_report: true for devices (e.g. "3Dconnexion Universal Receiver") that pack + // translation and rotation into a single 13-byte report ID 1, rather than reporting + // translation on ID 1 and rotation on ID 2 as separate 7-byte reports. + SpaceMousePlugin(const std::string& device_path, const std::string& collection_id, bool combined_report); + ~SpaceMousePlugin(); + + void update(); + +private: + bool open_device(); + void close_device(); + void push_current_state(); + + std::string device_path_; + int device_fd_ = -1; + + // Whether this device reports translation and rotation in a single combined + // 13-byte report (report ID 1, translation in bytes 1-6, rotation in bytes + // 7-12) rather than as two separate 7-byte reports (report ID 1 = translation, + // report ID 2 = rotation). Matches the "3Dconnexion Universal Receiver" + // quirk in Isaac Lab's Se3SpaceMouse/Se2SpaceMouse. + bool combined_report_ = false; + + std::array translation_{ 0.0f, 0.0f, 0.0f }; + std::array rotation_{ 0.0f, 0.0f, 0.0f }; + std::set pressed_buttons_; + + std::shared_ptr session_; + core::SchemaPusher pusher_; +}; + +} // namespace spacemouse +} // namespace plugins diff --git a/src/python/isaacteleop/deviceio/__init__.py b/src/python/isaacteleop/deviceio/__init__.py index 0f9cc8c71c..07219961bf 100644 --- a/src/python/isaacteleop/deviceio/__init__.py +++ b/src/python/isaacteleop/deviceio/__init__.py @@ -19,6 +19,7 @@ MessageChannelTracker, FrameMetadataTrackerOak, Generic3AxisPedalTracker, + SpaceMouseTracker, OgloTactileTracker, TensorPushTracker, JointStateTracker, @@ -49,6 +50,7 @@ StreamType, FrameMetadataOak, Generic3AxisPedalOutput, + SpaceMouseOutput, OgloGloveSample, ) @@ -60,6 +62,7 @@ "StreamType", "FrameMetadataOak", "Generic3AxisPedalOutput", + "SpaceMouseOutput", "OgloGloveSample", "ITracker", "HandTracker", @@ -69,6 +72,7 @@ "MessageChannelTracker", "FrameMetadataTrackerOak", "Generic3AxisPedalTracker", + "SpaceMouseTracker", "OgloTactileTracker", "TensorPushTracker", "JointStateTracker", diff --git a/src/python/isaacteleop/retargeters/__init__.py b/src/python/isaacteleop/retargeters/__init__.py index 316c9beacf..01a3cc9ac4 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 + - SpaceMouseToSe3RelRetargeter: SpaceMouse translation/rotation state -> relative EE delta control + - SpaceMouseGripperRetargeter: SpaceMouse left-button toggle -> gripper open/closed + - SpaceMouseToSe2Retargeter: SpaceMouse translation/rotation 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), + # .spacemouse_se3_retargeter (requires retargeters-lite extra: scipy) + "SpaceMouseToSe3RelRetargeter": ( + ".spacemouse_se3_retargeter", + "SpaceMouseToSe3RelRetargeter", + "retargeters-lite", + ), + "SpaceMouseToSe3RelRetargeterConfig": ( + ".spacemouse_se3_retargeter", + "SpaceMouseToSe3RelRetargeterConfig", + "retargeters-lite", + ), + "SpaceMouseGripperRetargeter": ( + ".spacemouse_se3_retargeter", + "SpaceMouseGripperRetargeter", + None, + ), + # .spacemouse_se2_retargeter + "SpaceMouseToSe2Retargeter": ( + ".spacemouse_se2_retargeter", + "SpaceMouseToSe2Retargeter", + None, + ), + "SpaceMouseToSe2RetargeterConfig": ( + ".spacemouse_se2_retargeter", + "SpaceMouseToSe2RetargeterConfig", + 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", + "SpaceMouseToSe3RelRetargeter", + "SpaceMouseToSe3RelRetargeterConfig", + "SpaceMouseGripperRetargeter", + "SpaceMouseToSe2Retargeter", + "SpaceMouseToSe2RetargeterConfig", # SO-101 5-DOF arm retargeters "SO101ClutchRetargeter", "SO101GripperRetargeter", diff --git a/src/python/isaacteleop/retargeters/spacemouse_se2_retargeter.py b/src/python/isaacteleop/retargeters/spacemouse_se2_retargeter.py new file mode 100644 index 0000000000..c47c49195b --- /dev/null +++ b/src/python/isaacteleop/retargeters/spacemouse_se2_retargeter.py @@ -0,0 +1,92 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +SpaceMouse SE2 Retargeter Module. + +Maps raw SpaceMouse translation/rotation 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 ( + SpaceMouseRotationType, + SpaceMouseTranslationType, +) +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 + + +@dataclass +class SpaceMouseToSe2RetargeterConfig: + """Configuration for the spacemouse-to-SE2 base-velocity retargeter.""" + + v_x_sensitivity: float = 1.0 + v_y_sensitivity: float = 1.0 + omega_z_sensitivity: float = 1.0 + + +class SpaceMouseToSe2Retargeter(BaseRetargeter): + """ + Maps raw SpaceMouse translation/rotation axes to a 3D base velocity command + (v_x, v_y, omega_z). + + Axis bindings (matching Isaac Lab's legacy Se2SpaceMouse): + Move mouse laterally: base v_x / v_y + Twist mouse about the z-axis: base omega_z + + Output is the instantaneous command implied by the current axis deflection + (scaled by sensitivity), not an integrated velocity -- matching a continuous-axis + input device. + """ + + def __init__(self, config: SpaceMouseToSe2RetargeterConfig, name: str) -> None: + self._config = config + super().__init__(name=name) + + def input_spec(self) -> RetargeterIOType: + return { + "spacemouse_translation": OptionalType(SpaceMouseTranslationType()), + "spacemouse_rotation": OptionalType(SpaceMouseRotationType()), + } + + 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"] + translation_in = inputs["spacemouse_translation"] + rotation_in = inputs["spacemouse_rotation"] + if translation_in.is_none or rotation_in.is_none: + base_command[0] = np.zeros(3, dtype=np.float32) + return + + translation = np.asarray(translation_in[0]) + rotation = np.asarray(rotation_in[0]) + + velocity = np.zeros(3) + velocity[1] = self._config.v_y_sensitivity * translation[0] + velocity[0] = self._config.v_x_sensitivity * translation[1] + velocity[2] = self._config.omega_z_sensitivity * rotation[1] + + base_command[0] = velocity.astype(np.float32) diff --git a/src/python/isaacteleop/retargeters/spacemouse_se3_retargeter.py b/src/python/isaacteleop/retargeters/spacemouse_se3_retargeter.py new file mode 100644 index 0000000000..a2c1f89095 --- /dev/null +++ b/src/python/isaacteleop/retargeters/spacemouse_se3_retargeter.py @@ -0,0 +1,153 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +SpaceMouse SE3 Retargeter Module. + +Maps raw SpaceMouse translation/rotation/button 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 ( + SpaceMouseButtonsType, + SpaceMouseRotationType, + SpaceMouseTranslationType, +) +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, +) + +# Button bit position matching Isaac Lab's legacy Se3SpaceMouse: the left button +# toggles the gripper. (Bit 1, the right button, requests a reset -- a session-level +# concern handled outside this retargeter, the same way it is for the keyboard's R key.) +BUTTON_LEFT = 0 + + +@dataclass +class SpaceMouseToSe3RelRetargeterConfig: + """Configuration for the spacemouse-to-SE3-relative retargeter.""" + + pos_sensitivity: float = 0.4 + rot_sensitivity: float = 0.8 + + +class SpaceMouseToSe3RelRetargeter(BaseRetargeter): + """ + Maps raw SpaceMouse translation/rotation axes to a 6D end-effector delta command. + + Axis bindings (matching Isaac Lab's legacy Se3SpaceMouse): + Move mouse laterally: x-y plane position delta + Move mouse vertically: z position delta (inverted) + Twist mouse about an axis: rotation delta about the corresponding axis + + Output is the instantaneous command implied by the current axis deflection + (scaled by sensitivity), not an integrated delta -- matching a continuous-axis + input device. + """ + + def __init__(self, config: SpaceMouseToSe3RelRetargeterConfig, name: str) -> None: + self._config = config + super().__init__(name=name) + + def input_spec(self) -> RetargeterIOType: + return { + "spacemouse_translation": OptionalType(SpaceMouseTranslationType()), + "spacemouse_rotation": OptionalType(SpaceMouseRotationType()), + } + + 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"] + translation_in = inputs["spacemouse_translation"] + rotation_in = inputs["spacemouse_rotation"] + if translation_in.is_none or rotation_in.is_none: + ee_delta[0] = np.zeros(6, dtype=np.float32) + return + + translation = np.asarray(translation_in[0]) + rotation = np.asarray(rotation_in[0]) + pos_sens = self._config.pos_sensitivity + rot_sens = self._config.rot_sensitivity + + delta_pos = np.zeros(3) + delta_pos[1] = pos_sens * translation[0] + delta_pos[0] = pos_sens * translation[1] + delta_pos[2] = -pos_sens * translation[2] + + delta_euler = np.zeros(3) + delta_euler[1] = rot_sens * rotation[0] + delta_euler[0] = rot_sens * rotation[1] + delta_euler[2] = -rot_sens * rotation[2] + + delta_rot = Rotation.from_euler("XYZ", delta_euler).as_rotvec() + + ee_delta[0] = np.concatenate([delta_pos, delta_rot]).astype(np.float32) + + +class SpaceMouseGripperRetargeter(BaseRetargeter): + """ + Toggles a gripper open/closed state on each rising edge of the left 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_left_pressed = False + + def input_spec(self) -> RetargeterIOType: + return {"spacemouse_buttons": OptionalType(SpaceMouseButtonsType())} + + def output_spec(self) -> RetargeterIOType: + return { + "gripper_command": TensorGroupType( + "gripper_command", [FloatType("command")] + ) + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + if context.execution_events.reset: + self._closed = False + self._prev_left_pressed = False + + gripper_out = outputs["gripper_command"] + buttons_in = inputs["spacemouse_buttons"] + if buttons_in.is_none: + gripper_out[0] = -1.0 if self._closed else 1.0 + return + + bitmap = np.asarray(buttons_in[0]) + left_pressed = bool(bitmap[BUTTON_LEFT]) + if left_pressed and not self._prev_left_pressed: + self._closed = not self._closed + self._prev_left_pressed = left_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..53adb9b891 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,12 @@ from .hands_source import HandsSource from .controllers_source import ControllersSource from .pedals_source import Generic3AxisPedalSource +from .spacemouse_source import ( + SpaceMouseButtonsType, + SpaceMouseRotationType, + SpaceMouseSource, + SpaceMouseTranslationType, +) from .joint_state_source import JointStateSource from .full_body_source import FullBodySource from .message_channel_source import MessageChannelSource @@ -26,12 +32,14 @@ HandPoseTrackedType, ControllerSnapshotTrackedType, Generic3AxisPedalOutputTrackedType, + SpaceMouseOutputTrackedType, JointStateOutputTrackedType, FullBodyPoseTrackedType, DeviceIOHeadPoseTracked, DeviceIOHandPoseTracked, DeviceIOControllerSnapshotTracked, DeviceIOGeneric3AxisPedalOutputTracked, + DeviceIOSpaceMouseOutputTracked, DeviceIOJointStateOutputTracked, DeviceIOFullBodyPoseTracked, MessageChannelMessagesTrackedType, @@ -49,6 +57,10 @@ "HandsSource", "ControllersSource", "Generic3AxisPedalSource", + "SpaceMouseButtonsType", + "SpaceMouseRotationType", + "SpaceMouseSource", + "SpaceMouseTranslationType", "JointStateSource", "FullBodySource", "MessageChannelSource", @@ -61,6 +73,7 @@ "HandPoseTrackedType", "ControllerSnapshotTrackedType", "Generic3AxisPedalOutputTrackedType", + "SpaceMouseOutputTrackedType", "JointStateOutputTrackedType", "FullBodyPoseTrackedType", "MessageChannelMessagesTrackedType", @@ -70,6 +83,7 @@ "DeviceIOHandPoseTracked", "DeviceIOControllerSnapshotTracked", "DeviceIOGeneric3AxisPedalOutputTracked", + "DeviceIOSpaceMouseOutputTracked", "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..c63b09e3a8 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 @@ -22,6 +22,7 @@ JointStateOutput, FullBodyPose, MessageChannelMessagesTracked, + SpaceMouseOutput, ) @@ -92,6 +93,12 @@ class Generic3AxisPedalOutputTrackedType(_PayloadTensorType): _payload_cls = Generic3AxisPedalOutput +class SpaceMouseOutputTrackedType(_PayloadTensorType): + """SpaceMouseOutput payload from DeviceIO SpaceMouseTracker.""" + + _payload_cls = SpaceMouseOutput + + class JointStateOutputTrackedType(_PayloadTensorType): """JointStateOutput payload from DeviceIO JointStateTracker.""" @@ -172,6 +179,18 @@ def DeviceIOGeneric3AxisPedalOutputTracked() -> TensorGroupType: ) +def DeviceIOSpaceMouseOutputTracked() -> TensorGroupType: + """Tracked spacemouse data from DeviceIO SpaceMouseTracker. + + Contains: + spacemouse_tracked: SpaceMouseOutput handle, or None when inactive + """ + return TensorGroupType( + "deviceio_spacemouse_output", + [SpaceMouseOutputTrackedType("spacemouse_tracked")], + ) + + def DeviceIOJointStateOutputTracked() -> TensorGroupType: """Tracked joint-state data from DeviceIO JointStateTracker. diff --git a/src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/spacemouse_source.py b/src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/spacemouse_source.py new file mode 100644 index 0000000000..ed0f38e94f --- /dev/null +++ b/src/python/isaacteleop/retargeting_engine/deviceio_source_nodes/spacemouse_source.py @@ -0,0 +1,202 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +SpaceMouse Source Node - DeviceIO to Retargeting Engine converter. + +Converts raw SpaceMouseOutput flatbuffer data (3Dconnexion HID axis/button state) to +three standard outputs: translation axes, rotation axes, and a button-press bitmap. +Carries no semantic mapping -- which axis/button means what (a position delta, a +rotation delta, 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 DeviceIOSpaceMouseOutputTracked +from .interface import IDeviceIOSource + +if TYPE_CHECKING: + from isaacteleop.deviceio import ITracker + from isaacteleop.schema import SpaceMouseOutput + +# Default collection_id matching the spacemouse plugin and SpaceMouseTracker. +DEFAULT_SPACEMOUSE_COLLECTION_ID = "spacemouse" + +# Fixed axis-array size for both translation and rotation: [x, y, z]. +SPACEMOUSE_AXES_SIZE = 3 + +# Button bitmap size: covers every bit position the plugin's button-report byte can set. +SPACEMOUSE_BUTTONS_BITMAP_SIZE = 8 + + +def SpaceMouseTranslationType() -> TensorGroupType: + """Type for the "spacemouse_translation" output: a 3-entry float32 [x, y, z] array.""" + return TensorGroupType( + "spacemouse_translation", + [ + NDArrayType( + "axes", + shape=(SPACEMOUSE_AXES_SIZE,), + dtype=DLDataType.FLOAT, + dtype_bits=32, + ) + ], + ) + + +def SpaceMouseRotationType() -> TensorGroupType: + """Type for the "spacemouse_rotation" output: a 3-entry float32 [x, y, z] array.""" + return TensorGroupType( + "spacemouse_rotation", + [ + NDArrayType( + "axes", + shape=(SPACEMOUSE_AXES_SIZE,), + dtype=DLDataType.FLOAT, + dtype_bits=32, + ) + ], + ) + + +def SpaceMouseButtonsType() -> TensorGroupType: + """Type for the "spacemouse_buttons" output: an 8-entry uint8 bitmap indexed by button index.""" + return TensorGroupType( + "spacemouse_buttons", + [ + NDArrayType( + "bitmap", + shape=(SPACEMOUSE_BUTTONS_BITMAP_SIZE,), + dtype=DLDataType.UINT, + dtype_bits=8, + ) + ], + ) + + +class SpaceMouseSource(IDeviceIOSource): + """ + Stateless converter: DeviceIO SpaceMouseOutput → translation / rotation / button tensors. + + Inputs: + - "deviceio_spacemouse": Raw SpaceMouseOutput flatbuffer from SpaceMouseTracker + + Outputs (Optional — absent when the spacemouse plugin has not yet streamed): + - "spacemouse_translation": OptionalTensorGroup, a 3-entry float32 [x, y, z] + array in [-1, 1]. + - "spacemouse_rotation": OptionalTensorGroup, a 3-entry float32 [x, y, z] + array in [-1, 1]. + - "spacemouse_buttons": OptionalTensorGroup, an 8-entry uint8 bitmap indexed + by button index (1 = held, 0 = released). + + Usage: + # In TeleopSession, the spacemouse tracker is discovered from the pipeline; + # data is polled via poll_tracker. Or manually: + state = spacemouse_tracker.get_spacemouse_data(session) + result = spacemouse_source_node({ + "deviceio_spacemouse": TensorGroup(DeviceIOSpaceMouseOutputTracked(), [state]) + }) + """ + + def __init__( + self, name: str, collection_id: str = DEFAULT_SPACEMOUSE_COLLECTION_ID + ) -> None: + """Initialize stateless spacemouse source node. + + Creates a SpaceMouseTracker instance for TeleopSession to discover and use. + + Args: + name: Unique name for this source node + collection_id: Tensor collection ID for spacemouse data (must match the spacemouse plugin). + """ + import isaacteleop.deviceio as deviceio + + self._spacemouse_tracker = deviceio.SpaceMouseTracker(collection_id) + self._collection_id = collection_id + super().__init__(name) + + def get_tracker(self) -> ITracker: + """Get the SpaceMouseTracker instance. + + Returns: + The SpaceMouseTracker instance for TeleopSession to initialize + """ + return self._spacemouse_tracker + + def poll_tracker(self, deviceio_session: Any) -> RetargeterIO: + """Poll the spacemouse tracker and return input data. + + Args: + deviceio_session: The active DeviceIO session. + + Returns: + Dict with "deviceio_spacemouse" TensorGroup containing SpaceMouseOutput | None. + """ + state = self._spacemouse_tracker.get_spacemouse_data(deviceio_session) + tg = TensorGroup(DeviceIOSpaceMouseOutputTracked()) + tg[0] = state + return {"deviceio_spacemouse": tg} + + def input_spec(self) -> RetargeterIOType: + """Declare DeviceIO spacemouse input.""" + return { + "deviceio_spacemouse": DeviceIOSpaceMouseOutputTracked(), + } + + def output_spec(self) -> RetargeterIOType: + """Declare standard spacemouse outputs (Optional — may be absent).""" + return { + "spacemouse_translation": OptionalType(SpaceMouseTranslationType()), + "spacemouse_rotation": OptionalType(SpaceMouseRotationType()), + "spacemouse_buttons": OptionalType(SpaceMouseButtonsType()), + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + """ + Convert DeviceIO SpaceMouseOutput to the standard spacemouse outputs. + + Calls ``set_none()`` on all three outputs when the spacemouse plugin has not + yet streamed. + + Args: + inputs: Dict with "deviceio_spacemouse" containing SpaceMouseOutput | None + outputs: Dict with "spacemouse_translation", "spacemouse_rotation", and + "spacemouse_buttons" OptionalTensorGroups + context: Shared ComputeContext for the current step (carries GraphTime). + """ + import numpy as np + + state: SpaceMouseOutput | None = inputs["deviceio_spacemouse"][0] + + translation_out = outputs["spacemouse_translation"] + rotation_out = outputs["spacemouse_rotation"] + buttons_out = outputs["spacemouse_buttons"] + if state is None: + translation_out.set_none() + rotation_out.set_none() + buttons_out.set_none() + return + + translation = np.zeros(SPACEMOUSE_AXES_SIZE, dtype=np.float32) + reported_translation = np.asarray(state.translation, dtype=np.float32) + count = min(reported_translation.shape[0], SPACEMOUSE_AXES_SIZE) + translation[:count] = reported_translation[:count] + translation_out[0] = translation + + rotation = np.zeros(SPACEMOUSE_AXES_SIZE, dtype=np.float32) + reported_rotation = np.asarray(state.rotation, dtype=np.float32) + count = min(reported_rotation.shape[0], SPACEMOUSE_AXES_SIZE) + rotation[:count] = reported_rotation[:count] + rotation_out[0] = rotation + + bitmap = np.zeros(SPACEMOUSE_BUTTONS_BITMAP_SIZE, dtype=np.uint8) + for code in state.pressed_buttons: + if code < SPACEMOUSE_BUTTONS_BITMAP_SIZE: + bitmap[code] = 1 + buttons_out[0] = bitmap diff --git a/src/python/isaacteleop/schema/__init__.py b/src/python/isaacteleop/schema/__init__.py index 39829651d5..b7a61a099e 100644 --- a/src/python/isaacteleop/schema/__init__.py +++ b/src/python/isaacteleop/schema/__init__.py @@ -38,6 +38,9 @@ # Pedals-related types. Generic3AxisPedalOutput, Generic3AxisPedalOutputRecord, + # SpaceMouse types (raw 3Dconnexion axis/button state). + SpaceMouseOutput, + SpaceMouseOutputRecord, # OGLO tactile glove types. OgloGloveSample, OgloGloveSampleRecord, @@ -123,6 +126,9 @@ def __getattr__(name: str): # Pedals types. "Generic3AxisPedalOutput", "Generic3AxisPedalOutputRecord", + # SpaceMouse types (raw 3Dconnexion axis/button state). + "SpaceMouseOutput", + "SpaceMouseOutputRecord", # OGLO tactile glove types. "OgloGloveSample", "OgloGloveSampleRecord", diff --git a/tests/python/core/retargeting_engine/test_spacemouse_retargeter.py b/tests/python/core/retargeting_engine/test_spacemouse_retargeter.py new file mode 100644 index 0000000000..8b7b8a3222 --- /dev/null +++ b/tests/python/core/retargeting_engine/test_spacemouse_retargeter.py @@ -0,0 +1,188 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Sim-free unit tests for SpaceMouseToSe3RelRetargeter, SpaceMouseGripperRetargeter, and +SpaceMouseToSe2Retargeter, exercised through SpaceMouseSource 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 SpaceMouseSource +from isaacteleop.retargeting_engine.interface.base_retargeter import _make_output_group +from isaacteleop.retargeting_engine.interface.tensor_group import TensorGroup +from isaacteleop.retargeters import ( + SpaceMouseGripperRetargeter, + SpaceMouseToSe2Retargeter, + SpaceMouseToSe2RetargeterConfig, + SpaceMouseToSe3RelRetargeter, + SpaceMouseToSe3RelRetargeterConfig, +) +from isaacteleop.schema import SpaceMouseOutput + +BUTTON_LEFT = 0 + + +def _spacemouse_source(): + return SpaceMouseSource(name="spacemouse") + + +def _run_source( + src, + translation: list[float] | None, + rotation: list[float] | None = None, + pressed_buttons: list[int] | None = None, +): + """Feed raw axis/button state (None translation = inactive device) through SpaceMouseSource.compute().""" + state = ( + None + if translation is None + else SpaceMouseOutput( + translation, rotation or [0.0, 0.0, 0.0], pressed_buttons or [], True + ) + ) + + input_spec = src.input_spec() + tg = TensorGroup(input_spec["deviceio_spacemouse"]) + tg[0] = state + + outputs = {name: _make_output_group(gt) for name, gt in src.output_spec().items()} + src.compute({"deviceio_spacemouse": tg}, outputs) + return outputs + + +class TestSpaceMouseToSe3RelRetargeter: + def test_translation_produces_position_delta(self): + """translation[0] -> +Y, translation[1] -> +X, translation[2] -> -Z (inverted).""" + src = _spacemouse_source() + src_outputs = _run_source(src, translation=[1.0, 1.0, 1.0]) + + retargeter = SpaceMouseToSe3RelRetargeter( + SpaceMouseToSe3RelRetargeterConfig(), name="se3" + ) + out = {"ee_delta": _make_output_group(retargeter.output_spec()["ee_delta"])} + retargeter.compute( + { + "spacemouse_translation": src_outputs["spacemouse_translation"], + "spacemouse_rotation": src_outputs["spacemouse_rotation"], + }, + out, + ) + + delta = np.asarray(out["ee_delta"][0]) + assert delta[0] == pytest.approx(0.4) # +X from translation[1] + assert delta[1] == pytest.approx(0.4) # +Y from translation[0] + assert delta[2] == pytest.approx(-0.4) # -Z from translation[2] (inverted) + + def test_inactive_device_yields_zero_delta(self): + src = _spacemouse_source() + src_outputs = _run_source(src, translation=None) + + se3 = SpaceMouseToSe3RelRetargeter( + SpaceMouseToSe3RelRetargeterConfig(), name="se3" + ) + se3_out = {"ee_delta": _make_output_group(se3.output_spec()["ee_delta"])} + se3.compute( + { + "spacemouse_translation": src_outputs["spacemouse_translation"], + "spacemouse_rotation": src_outputs["spacemouse_rotation"], + }, + se3_out, + ) + assert np.allclose(np.asarray(se3_out["ee_delta"][0]), 0.0) + + +class TestSpaceMouseGripperRetargeter: + def test_gripper_toggles_on_button_rising_edge_only(self): + """Left-button press/release/press across three frames toggles on each rising edge.""" + src = _spacemouse_source() + retargeter = SpaceMouseGripperRetargeter(name="gripper") + + def step(pressed_buttons): + src_outputs = _run_source( + src, translation=[0.0, 0.0, 0.0], pressed_buttons=pressed_buttons + ) + out = { + "gripper_command": _make_output_group( + retargeter.output_spec()["gripper_command"] + ) + } + retargeter.compute( + {"spacemouse_buttons": src_outputs["spacemouse_buttons"]}, out + ) + return float(out["gripper_command"][0]) + + assert step([]) == pytest.approx(1.0) # open (default) + assert step([BUTTON_LEFT]) == pytest.approx(-1.0) # rising edge -> close + assert step([BUTTON_LEFT]) == pytest.approx( + -1.0 + ) # held -> stays closed, no re-toggle + assert step([]) == pytest.approx(-1.0) # release -> stays closed + assert step([BUTTON_LEFT]) == pytest.approx(1.0) # rising edge again -> open + + def test_inactive_device_yields_default_open(self): + src = _spacemouse_source() + src_outputs = _run_source(src, translation=None) + + gripper = SpaceMouseGripperRetargeter(name="gripper") + gripper_out = { + "gripper_command": _make_output_group( + gripper.output_spec()["gripper_command"] + ) + } + gripper.compute( + {"spacemouse_buttons": src_outputs["spacemouse_buttons"]}, gripper_out + ) + assert float(gripper_out["gripper_command"][0]) == pytest.approx( + 1.0 + ) # default open + + +class TestSpaceMouseToSe2Retargeter: + def test_translation_and_rotation_combine(self): + """translation -> v_x/v_y, rotation[1] -> omega_z.""" + src = _spacemouse_source() + src_outputs = _run_source( + src, translation=[1.0, 1.0, 0.0], rotation=[0.0, 1.0, 0.0] + ) + + retargeter = SpaceMouseToSe2Retargeter( + SpaceMouseToSe2RetargeterConfig(), name="se2" + ) + out = { + "base_command": _make_output_group(retargeter.output_spec()["base_command"]) + } + retargeter.compute( + { + "spacemouse_translation": src_outputs["spacemouse_translation"], + "spacemouse_rotation": src_outputs["spacemouse_rotation"], + }, + out, + ) + + velocity = np.asarray(out["base_command"][0]) + assert velocity[0] == pytest.approx(1.0) # v_x from translation[1] + assert velocity[1] == pytest.approx(1.0) # v_y from translation[0] + assert velocity[2] == pytest.approx(1.0) # omega_z from rotation[1] + + def test_inactive_device_yields_zero_velocity(self): + src = _spacemouse_source() + src_outputs = _run_source(src, translation=None) + + retargeter = SpaceMouseToSe2Retargeter( + SpaceMouseToSe2RetargeterConfig(), name="se2" + ) + out = { + "base_command": _make_output_group(retargeter.output_spec()["base_command"]) + } + retargeter.compute( + { + "spacemouse_translation": src_outputs["spacemouse_translation"], + "spacemouse_rotation": src_outputs["spacemouse_rotation"], + }, + out, + ) + + assert np.allclose(np.asarray(out["base_command"][0]), 0.0) diff --git a/tests/python/core/retargeting_engine/test_spacemouse_source.py b/tests/python/core/retargeting_engine/test_spacemouse_source.py new file mode 100644 index 0000000000..924654a211 --- /dev/null +++ b/tests/python/core/retargeting_engine/test_spacemouse_source.py @@ -0,0 +1,86 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Tests for the SpaceMouseSource DeviceIO converter. + +Exercises the stateless converter from a raw ``SpaceMouseOutput`` FlatBuffer (constructed +via the real schema Python bindings) into translation/rotation axis arrays and a button-press +bitmap, with no OpenXR device involved. +""" + +import numpy as np +import pytest + +from isaacteleop.retargeting_engine.deviceio_source_nodes import SpaceMouseSource +from isaacteleop.retargeting_engine.interface.base_retargeter import _make_output_group +from isaacteleop.retargeting_engine.interface.tensor_group import TensorGroup +from isaacteleop.schema import SpaceMouseOutput + +BUTTON_LEFT = 0 + + +def _spacemouse_source(): + return SpaceMouseSource(name="spacemouse") + + +def _run_source( + src, + translation: list[float] | None, + rotation: list[float] | None = None, + pressed_buttons: list[int] | None = None, +): + """Feed raw axis/button state (None translation = inactive device) through SpaceMouseSource.compute().""" + state = ( + None + if translation is None + else SpaceMouseOutput( + translation, rotation or [0.0, 0.0, 0.0], pressed_buttons or [], True + ) + ) + + input_spec = src.input_spec() + tg = TensorGroup(input_spec["deviceio_spacemouse"]) + tg[0] = state + + outputs = {name: _make_output_group(gt) for name, gt in src.output_spec().items()} + src.compute({"deviceio_spacemouse": tg}, outputs) + return outputs + + +class TestSpaceMouseSource: + def test_source_creates_real_tracker(self): + src = _spacemouse_source() + tracker = src.get_tracker() + assert tracker is not None + assert tracker.get_name() == "SpaceMouseTracker" + + def test_translation_and_rotation_pass_through(self): + src = _spacemouse_source() + outputs = _run_source( + src, translation=[0.1, 0.2, 0.3], rotation=[-0.1, -0.2, -0.3] + ) + + assert not outputs["spacemouse_translation"].is_none + assert not outputs["spacemouse_rotation"].is_none + translation = np.asarray(outputs["spacemouse_translation"][0]) + rotation = np.asarray(outputs["spacemouse_rotation"][0]) + assert translation == pytest.approx([0.1, 0.2, 0.3]) + assert rotation == pytest.approx([-0.1, -0.2, -0.3]) + + def test_button_marks_bitmap(self): + src = _spacemouse_source() + outputs = _run_source( + src, translation=[0.0, 0.0, 0.0], pressed_buttons=[BUTTON_LEFT] + ) + + assert not outputs["spacemouse_buttons"].is_none + bitmap = np.asarray(outputs["spacemouse_buttons"][0]) + assert bitmap[BUTTON_LEFT] == 1 + assert bitmap.sum() == 1 + + def test_inactive_device_yields_none(self): + src = _spacemouse_source() + outputs = _run_source(src, translation=None) + assert outputs["spacemouse_translation"].is_none + assert outputs["spacemouse_rotation"].is_none + assert outputs["spacemouse_buttons"].is_none diff --git a/tests/python/core/schema/test_spacemouse.py b/tests/python/core/schema/test_spacemouse.py new file mode 100644 index 0000000000..1333e5c6ba --- /dev/null +++ b/tests/python/core/schema/test_spacemouse.py @@ -0,0 +1,123 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Unit tests for SpaceMouseOutput type in isaacteleop.schema. + +Tests the following FlatBuffers types: +- SpaceMouseOutput: Table with translation, rotation, pressed_buttons, is_valid +- SpaceMouseOutputRecord: Record wrapper carrying DeviceDataTimestamp + +Timestamps are carried by SpaceMouseOutputRecord, not SpaceMouseOutput. +""" + +import pytest + +from isaacteleop.schema import ( + DeviceDataTimestamp, + SpaceMouseOutput, + SpaceMouseOutputRecord, +) + + +class TestSpaceMouseOutputConstruction: + """Tests for SpaceMouseOutput table construction.""" + + def test_construction(self): + """Test construction with explicit fields.""" + output = SpaceMouseOutput( + translation=[0.1, 0.2, 0.3], + rotation=[-0.1, -0.2, -0.3], + pressed_buttons=[0], + is_valid=True, + ) + + assert list(output.translation) == pytest.approx([0.1, 0.2, 0.3]) + assert list(output.rotation) == pytest.approx([-0.1, -0.2, -0.3]) + assert list(output.pressed_buttons) == [0] + assert output.is_valid is True + + def test_repr(self): + """Test __repr__ returns meaningful string.""" + output = SpaceMouseOutput( + translation=[], rotation=[], pressed_buttons=[], is_valid=False + ) + repr_str = repr(output) + + assert "SpaceMouseOutput" in repr_str + + +class TestSpaceMouseOutputFields: + """Tests that translation, rotation, and pressed_buttons round-trip through the encoding.""" + + def test_empty_fields(self): + """Test encoding with no axes and no buttons held.""" + output = SpaceMouseOutput( + translation=[], rotation=[], pressed_buttons=[], is_valid=True + ) + + assert list(output.translation) == [] + assert list(output.rotation) == [] + assert list(output.pressed_buttons) == [] + + def test_encodings_are_independent(self): + """Test each encoding carries its own values, not a shared buffer's.""" + first = SpaceMouseOutput( + translation=[1.0], rotation=[], pressed_buttons=[0], is_valid=True + ) + second = SpaceMouseOutput( + translation=[-1.0], rotation=[], pressed_buttons=[1], is_valid=True + ) + + assert list(first.translation) == pytest.approx([1.0]) + assert list(second.translation) == pytest.approx([-1.0]) + assert list(first.pressed_buttons) == [0] + assert list(second.pressed_buttons) == [1] + + +class TestSpaceMouseOutputEncoding: + """Tests that an encoded payload reads back. + + A tracker with no spacemouse 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 = SpaceMouseOutput( + translation=[0.5], rotation=[0.25], pressed_buttons=[0], is_valid=True + ) + + assert output + assert list(output.translation) == pytest.approx([0.5]) + assert list(output.rotation) == pytest.approx([0.25]) + + def test_repr_present(self): + """Repr of a present payload names the type.""" + assert "SpaceMouseOutput" in repr( + SpaceMouseOutput( + translation=[], rotation=[], pressed_buttons=[], is_valid=True + ) + ) + + +class TestSpaceMouseOutputRecordTimestamp: + """Tests for SpaceMouseOutputRecord with DeviceDataTimestamp.""" + + def test_construction_with_timestamp(self): + """Test SpaceMouseOutputRecord carries DeviceDataTimestamp.""" + data = SpaceMouseOutput( + translation=[0.5], rotation=[0.25], pressed_buttons=[0], is_valid=True + ) + ts = DeviceDataTimestamp(1000000000, 2000000000, 3000000000) + record = SpaceMouseOutputRecord(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.translation) == pytest.approx([0.5]) + + def test_payload_less_record(self): + """A record may carry a timestamp and no payload: MCAP's frame sentinel.""" + record = SpaceMouseOutputRecord(None, DeviceDataTimestamp(1, 2, 3)) + assert record.data is None + assert record.timestamp.available_time_local_common_clock == 1 From 119d1f65f5b1d69eedeefb6526c5a6b1f351952f Mon Sep 17 00:00:00 2001 From: Rafael Wiltz Date: Tue, 25 Aug 2026 11:09:47 -0400 Subject: [PATCH 2/8] Document Se2 omega_z sign asymmetry, add multi-button bitmap test Signed-off-by: Rafael Wiltz --- .../retargeters/spacemouse_se2_retargeter.py | 2 ++ .../retargeting_engine/test_spacemouse_source.py | 15 +++++++++++++++ 2 files changed, 17 insertions(+) diff --git a/src/python/isaacteleop/retargeters/spacemouse_se2_retargeter.py b/src/python/isaacteleop/retargeters/spacemouse_se2_retargeter.py index c47c49195b..eba0664508 100644 --- a/src/python/isaacteleop/retargeters/spacemouse_se2_retargeter.py +++ b/src/python/isaacteleop/retargeters/spacemouse_se2_retargeter.py @@ -87,6 +87,8 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N velocity = np.zeros(3) velocity[1] = self._config.v_y_sensitivity * translation[0] velocity[0] = self._config.v_x_sensitivity * translation[1] + # Unlike Se3's yaw, omega_z is not sign-inverted here -- a faithful port of the + # legacy Se2SpaceMouse's (likely unintentional) asymmetry with Se3's rotation mapping. velocity[2] = self._config.omega_z_sensitivity * rotation[1] base_command[0] = velocity.astype(np.float32) diff --git a/tests/python/core/retargeting_engine/test_spacemouse_source.py b/tests/python/core/retargeting_engine/test_spacemouse_source.py index 924654a211..b50a2ba244 100644 --- a/tests/python/core/retargeting_engine/test_spacemouse_source.py +++ b/tests/python/core/retargeting_engine/test_spacemouse_source.py @@ -17,6 +17,7 @@ from isaacteleop.schema import SpaceMouseOutput BUTTON_LEFT = 0 +BUTTON_RIGHT = 1 def _spacemouse_source(): @@ -78,6 +79,20 @@ def test_button_marks_bitmap(self): assert bitmap[BUTTON_LEFT] == 1 assert bitmap.sum() == 1 + def test_multiple_buttons_mark_bitmap(self): + src = _spacemouse_source() + outputs = _run_source( + src, + translation=[0.0, 0.0, 0.0], + pressed_buttons=[BUTTON_LEFT, BUTTON_RIGHT], + ) + + assert not outputs["spacemouse_buttons"].is_none + bitmap = np.asarray(outputs["spacemouse_buttons"][0]) + assert bitmap[BUTTON_LEFT] == 1 + assert bitmap[BUTTON_RIGHT] == 1 + assert bitmap.sum() == 2 + def test_inactive_device_yields_none(self): src = _spacemouse_source() outputs = _run_source(src, translation=None) From 42f12495fa66852c8408287c1bcfe0d05462f4a4 Mon Sep 17 00:00:00 2001 From: Rafael Wiltz Date: Tue, 25 Aug 2026 12:46:33 -0400 Subject: [PATCH 3/8] Add spacemouse printer example Signed-off-by: Rafael Wiltz --- .../python/spacemouse_printer_example.py | 132 ++++++++++++++++++ 1 file changed, 132 insertions(+) create mode 100644 examples/teleop/python/spacemouse_printer_example.py diff --git a/examples/teleop/python/spacemouse_printer_example.py b/examples/teleop/python/spacemouse_printer_example.py new file mode 100644 index 0000000000..28b1ec1702 --- /dev/null +++ b/examples/teleop/python/spacemouse_printer_example.py @@ -0,0 +1,132 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +SpaceMouse Printer Example. + +Prints the translation axes, rotation axes, and every currently-held button each +frame, via SpaceMouseSource's "spacemouse_translation", "spacemouse_rotation", and +"spacemouse_buttons" outputs. Carries no semantic mapping (a position delta, a +rotation delta, a gripper toggle) -- that belongs in a retargeter (e.g. +SpaceMouseToSe3RelRetargeter) consuming this source's output. The spacemouse 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 SpaceMouseSource +from isaacteleop.teleop_session_manager import ( + TeleopSession, + TeleopSessionConfig, + PluginConfig, +) + + +PLUGIN_ROOT_DIR = Path(__file__).resolve().parent.parent.parent.parent / "plugins" +PLUGIN_NAME = "spacemouse" +PLUGIN_ROOT_ID = "spacemouse" + + +def main(): + import argparse + + parser = argparse.ArgumentParser(description=__doc__) + CloudXRLauncher.add_launcher_arguments(parser) + args = parser.parse_args() + + print("\n" + "=" * 80) + print(" SpaceMouse Printer Example") + print("=" * 80) + print("Move or twist the connected SpaceMouse, or press a button.") + print("=" * 80 + "\n") + + # ================================================================== + # Setup: Create spacemouse source + # ================================================================== + spacemouse_source = SpaceMouseSource(name="spacemouse") + + # ================================================================== + # 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="SpaceMousePrinterExample", + trackers=[], + pipeline=spacemouse_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() + translation_group = result["spacemouse_translation"] + rotation_group = result["spacemouse_rotation"] + buttons_group = result["spacemouse_buttons"] + + elapsed = session.get_elapsed_time() + if translation_group.is_none: + print( + f"[{elapsed:5.1f}s] (no spacemouse data yet)", + end="\r", + flush=True, + ) + time.sleep(0.01) + continue + + translation = translation_group[0] + rotation = rotation_group[0] + bitmap = buttons_group[0] + pressed = {code for code in range(len(bitmap)) if bitmap[code]} + + t_str = " ".join(f"{v:+.2f}" for v in translation) + r_str = " ".join(f"{v:+.2f}" for v in rotation) + + # Live status line (overwritten each frame). + names = [f"btn{code}" for code in sorted(pressed)] + print( + f"[{elapsed:5.1f}s] T: [{t_str}] R: [{r_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()) From 03dbd6fe5b7009f490b1c622ca85ce97d26d0d8d Mon Sep 17 00:00:00 2001 From: Rafael Wiltz Date: Tue, 25 Aug 2026 13:15:54 -0400 Subject: [PATCH 4/8] Reset motion state and guard destructor against double-close on device disconnect Signed-off-by: Rafael Wiltz --- src/plugins/spacemouse/spacemouse_plugin.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/plugins/spacemouse/spacemouse_plugin.cpp b/src/plugins/spacemouse/spacemouse_plugin.cpp index fe57a7eaae..cd5b2598ca 100644 --- a/src/plugins/spacemouse/spacemouse_plugin.cpp +++ b/src/plugins/spacemouse/spacemouse_plugin.cpp @@ -65,7 +65,8 @@ SpaceMousePlugin::SpaceMousePlugin(const std::string& device_path, const std::st SpaceMousePlugin::~SpaceMousePlugin() { - close_device(); + if (device_fd_ >= 0) + close_device(); } void SpaceMousePlugin::update() @@ -172,8 +173,11 @@ void SpaceMousePlugin::close_device() 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. + // A closed device can no longer report releases or motion -- forget everything + // it last reported so a stale button doesn't stick "pressed" and stale nonzero + // axes don't keep commanding motion after disconnect. + translation_.fill(0.0f); + rotation_.fill(0.0f); pressed_buttons_.clear(); } From 2816d6225d5eacbe367633c2a0f5bda505608003 Mon Sep 17 00:00:00 2001 From: Rafael Wiltz Date: Tue, 25 Aug 2026 13:15:59 -0400 Subject: [PATCH 5/8] Fix gripper spurious toggle when left button is held across a reset frame Signed-off-by: Rafael Wiltz --- .../retargeters/spacemouse_se3_retargeter.py | 18 +++++++--- .../test_spacemouse_retargeter.py | 34 +++++++++++++++++++ 2 files changed, 47 insertions(+), 5 deletions(-) diff --git a/src/python/isaacteleop/retargeters/spacemouse_se3_retargeter.py b/src/python/isaacteleop/retargeters/spacemouse_se3_retargeter.py index a2c1f89095..c9d6e93144 100644 --- a/src/python/isaacteleop/retargeters/spacemouse_se3_retargeter.py +++ b/src/python/isaacteleop/retargeters/spacemouse_se3_retargeter.py @@ -134,18 +134,26 @@ def output_spec(self) -> RetargeterIOType: } def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + gripper_out = outputs["gripper_command"] + buttons_in = inputs["spacemouse_buttons"] + left_pressed = ( + False + if buttons_in.is_none + else bool(np.asarray(buttons_in[0])[BUTTON_LEFT]) + ) + if context.execution_events.reset: self._closed = False - self._prev_left_pressed = False + # Sync to the current button state without toggling -- the left button + # may already be held on a reset frame, and that isn't a rising edge. + self._prev_left_pressed = left_pressed + gripper_out[0] = -1.0 if self._closed else 1.0 + return - gripper_out = outputs["gripper_command"] - buttons_in = inputs["spacemouse_buttons"] if buttons_in.is_none: gripper_out[0] = -1.0 if self._closed else 1.0 return - bitmap = np.asarray(buttons_in[0]) - left_pressed = bool(bitmap[BUTTON_LEFT]) if left_pressed and not self._prev_left_pressed: self._closed = not self._closed self._prev_left_pressed = left_pressed diff --git a/tests/python/core/retargeting_engine/test_spacemouse_retargeter.py b/tests/python/core/retargeting_engine/test_spacemouse_retargeter.py index 8b7b8a3222..2e2eea81da 100644 --- a/tests/python/core/retargeting_engine/test_spacemouse_retargeter.py +++ b/tests/python/core/retargeting_engine/test_spacemouse_retargeter.py @@ -12,6 +12,10 @@ from isaacteleop.retargeting_engine.deviceio_source_nodes import SpaceMouseSource 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 ( SpaceMouseGripperRetargeter, @@ -139,6 +143,36 @@ def test_inactive_device_yields_default_open(self): 1.0 ) # default open + def test_reset_does_not_toggle_gripper_while_left_is_held(self): + """Left button held across a reset frame is not a rising edge and must not toggle.""" + src = _spacemouse_source() + retargeter = SpaceMouseGripperRetargeter(name="gripper") + + def step(pressed_buttons, reset=False): + src_outputs = _run_source( + src, translation=[0.0, 0.0, 0.0], pressed_buttons=pressed_buttons + ) + out = { + "gripper_command": _make_output_group( + retargeter.output_spec()["gripper_command"] + ) + } + context = ComputeContext(execution_events=ExecutionEvents(reset=reset)) + retargeter.compute( + {"spacemouse_buttons": src_outputs["spacemouse_buttons"]}, out, context + ) + return float(out["gripper_command"][0]) + + assert step([BUTTON_LEFT]) == pytest.approx(-1.0) # rising edge -> close + # Reset resets the gripper to open, but the left button is still held -- not + # a new rising edge, so this must not immediately re-close it. + assert step([BUTTON_LEFT], reset=True) == pytest.approx(1.0) + assert step([BUTTON_LEFT]) == pytest.approx(1.0) # still held -> stays open + assert step([]) == pytest.approx(1.0) # release + assert step([BUTTON_LEFT]) == pytest.approx( + -1.0 + ) # genuine rising edge -> close + class TestSpaceMouseToSe2Retargeter: def test_translation_and_rotation_combine(self): From c132d6692eb9d5c3845ea5c2536b655dcb1800db Mon Sep 17 00:00:00 2001 From: Rafael Wiltz Date: Tue, 25 Aug 2026 13:28:00 -0400 Subject: [PATCH 6/8] Declare the retargeters-lite extra for SpaceMouseGripperRetargeter Signed-off-by: Rafael Wiltz --- src/python/isaacteleop/retargeters/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/python/isaacteleop/retargeters/__init__.py b/src/python/isaacteleop/retargeters/__init__.py index 01a3cc9ac4..faf5e4843d 100644 --- a/src/python/isaacteleop/retargeters/__init__.py +++ b/src/python/isaacteleop/retargeters/__init__.py @@ -122,7 +122,7 @@ "SpaceMouseGripperRetargeter": ( ".spacemouse_se3_retargeter", "SpaceMouseGripperRetargeter", - None, + "retargeters-lite", ), # .spacemouse_se2_retargeter "SpaceMouseToSe2Retargeter": ( From 1061f762c49e0e79e029fa4c22ad17016a7e7053 Mon Sep 17 00:00:00 2001 From: Rafael Wiltz Date: Tue, 25 Aug 2026 14:22:14 -0400 Subject: [PATCH 7/8] Preserve gripper edge state on reset frames with no spacemouse data Signed-off-by: Rafael Wiltz --- .../retargeters/spacemouse_se3_retargeter.py | 6 +++- .../test_spacemouse_retargeter.py | 31 +++++++++++++++++++ 2 files changed, 36 insertions(+), 1 deletion(-) diff --git a/src/python/isaacteleop/retargeters/spacemouse_se3_retargeter.py b/src/python/isaacteleop/retargeters/spacemouse_se3_retargeter.py index c9d6e93144..f98a2016a0 100644 --- a/src/python/isaacteleop/retargeters/spacemouse_se3_retargeter.py +++ b/src/python/isaacteleop/retargeters/spacemouse_se3_retargeter.py @@ -146,7 +146,11 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N self._closed = False # Sync to the current button state without toggling -- the left button # may already be held on a reset frame, and that isn't a rising edge. - self._prev_left_pressed = left_pressed + # Leave _prev_left_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_left_pressed = left_pressed gripper_out[0] = -1.0 if self._closed else 1.0 return diff --git a/tests/python/core/retargeting_engine/test_spacemouse_retargeter.py b/tests/python/core/retargeting_engine/test_spacemouse_retargeter.py index 2e2eea81da..3e9532cd73 100644 --- a/tests/python/core/retargeting_engine/test_spacemouse_retargeter.py +++ b/tests/python/core/retargeting_engine/test_spacemouse_retargeter.py @@ -173,6 +173,37 @@ def step(pressed_buttons, reset=False): -1.0 ) # genuine rising edge -> close + def test_reset_with_inactive_device_preserves_prior_edge_state(self): + """A reset frame with no spacemouse data must not clobber _prev_left_pressed.""" + src = _spacemouse_source() + retargeter = SpaceMouseGripperRetargeter(name="gripper") + + def step(translation, pressed_buttons=None, reset=False): + src_outputs = _run_source( + src, translation=translation, pressed_buttons=pressed_buttons + ) + out = { + "gripper_command": _make_output_group( + retargeter.output_spec()["gripper_command"] + ) + } + context = ComputeContext(execution_events=ExecutionEvents(reset=reset)) + retargeter.compute( + {"spacemouse_buttons": src_outputs["spacemouse_buttons"]}, out, context + ) + return float(out["gripper_command"][0]) + + assert step([0.0, 0.0, 0.0], [BUTTON_LEFT]) == pytest.approx( + -1.0 + ) # rising edge -> close + # Reset while the device is inactive (spacemouse_buttons.is_none) -- must + # not force _prev_left_pressed to False, or the next frame (left button + # still held) would be misread as a fresh rising edge. + assert step(None, reset=True) == pytest.approx(1.0) # gripper still resets + assert step([0.0, 0.0, 0.0], [BUTTON_LEFT]) == pytest.approx( + 1.0 + ) # still held -> no spurious toggle + class TestSpaceMouseToSe2Retargeter: def test_translation_and_rotation_combine(self): From 3b29a9f894b3fdf1eb9afe1db1540babc94d7fe0 Mon Sep 17 00:00:00 2001 From: Rafael Wiltz Date: Tue, 25 Aug 2026 16:03:07 -0400 Subject: [PATCH 8/8] Trim implementation notes from SpaceMouseOutputRecord schema comment The Record-type comment described how trackers/McapRecorder serialize and query data, which is implementation detail that doesn't belong in the wire schema. Keep the comment to what the type is. Signed-off-by: Rafael Wiltz --- src/core/schema/fbs/spacemouse.fbs | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/core/schema/fbs/spacemouse.fbs b/src/core/schema/fbs/spacemouse.fbs index 70e86217ce..b69589b2ca 100644 --- a/src/core/schema/fbs/spacemouse.fbs +++ b/src/core/schema/fbs/spacemouse.fbs @@ -34,10 +34,6 @@ table SpaceMouseOutput { } // MCAP recording wrapper for SpaceMouseOutput. -// -// Record types are the root types written to MCAP channels by the McapRecorder. -// Trackers serialize into Record types via their serialize() method, but the -// public query API returns the inner data type directly. table SpaceMouseOutputRecord { data: SpaceMouseOutput (id: 0); timestamp: DeviceDataTimestamp (id: 1);