Merge changes for adding xsens fullbody support to Nvidia Isaac Teleo…

CMakeLists.txt

@@ -187,6 +187,7 @@ if(BUILD_PLUGINS)
     add_subdirectory(src/plugins/vive_se3_tracker)
     add_subdirectory(src/plugins/controller_synthetic_hands)
     add_subdirectory(src/plugins/generic_3axis_pedal)
+    add_subdirectory(src/plugins/xsens_full_body)
     add_subdirectory(src/plugins/so101_leader)
     add_subdirectory(src/plugins/rebot_devarm_leader)
     add_subdirectory(src/plugins/manus)

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_full_body_tracker_noitom_impl.cpp
+    live_full_body_tracker_xsens_impl.cpp
     live_tensor_push_tracker_impl.cpp
     live_haptic_command_reader_tracker_impl.cpp
     ${GENERATED_TRACKER_LIVE_SOURCES}
@@ -24,6 +25,7 @@ add_library(live_trackers STATIC
     live_message_channel_tracker_impl.hpp
     live_full_body_tracker_pico_impl.hpp
     live_full_body_tracker_noitom_impl.hpp
+    live_full_body_tracker_xsens_impl.hpp
     live_tensor_push_tracker_impl.hpp
     live_haptic_command_reader_tracker_impl.hpp
 )

src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp

@@ -84,6 +84,7 @@ class LiveDeviceIOFactory
     std::unique_ptr<IMessageChannelTrackerImpl> create_message_channel_tracker_impl(const MessageChannelTracker* tracker);
     std::unique_ptr<IFullBodyTrackerImpl> create_full_body_tracker_pico_impl(const FullBodyTracker* tracker);
     std::unique_ptr<IFullBodyTrackerImpl> create_full_body_tracker_noitom_impl(const FullBodyTracker* tracker);
+    std::unique_ptr<IFullBodyTrackerImpl> create_full_body_tracker_xsens_impl(const FullBodyTracker* tracker);
     std::unique_ptr<ITensorPushTrackerImpl> create_tensor_push_tracker_impl(const TensorPushTracker* tracker);
     std::unique_ptr<IHapticCommandReaderTrackerImpl> create_haptic_command_reader_tracker_impl(
         const HapticCommandReaderTracker* tracker);

src/core/live_trackers/cpp/live_deviceio_factory.cpp

@@ -7,6 +7,7 @@
 #include "live_controller_tracker_impl.hpp"
 #include "live_full_body_tracker_noitom_impl.hpp"
 #include "live_full_body_tracker_pico_impl.hpp"
+#include "live_full_body_tracker_xsens_impl.hpp"
 #include "live_hand_tracker_impl.hpp"
 #include "live_haptic_command_reader_tracker_impl.hpp"
 #include "live_head_tracker_impl.hpp"
@@ -92,6 +93,12 @@ std::unique_ptr<ITrackerImpl> try_create_full_body_noitom_impl(LiveDeviceIOFacto
     return typed ? factory.create_full_body_tracker_noitom_impl(typed) : nullptr;
 }
 
+std::unique_ptr<ITrackerImpl> try_create_full_body_xsens_impl(LiveDeviceIOFactory& factory, const ITracker& tracker)
+{
+    auto* typed = dynamic_cast<const FullBodyTracker*>(&tracker);
+    return typed ? factory.create_full_body_tracker_xsens_impl(typed) : nullptr;
+}
+
 std::unique_ptr<ITrackerImpl> try_create_tensor_push_impl(LiveDeviceIOFactory& factory, const ITracker& tracker)
 {
     auto* typed = dynamic_cast<const TensorPushTracker*>(&tracker);
@@ -144,6 +151,8 @@ inline const TrackerDispatchEntry k_tracker_dispatch[] = {
     make_dispatch_entry<FullBodyTracker, LiveFullBodyTrackerPicoImpl>(&try_create_full_body_pico_impl, "body.pico-xr"),
     make_dispatch_entry<FullBodyTracker, LiveFullBodyTrackerNoitomImpl>(
         &try_create_full_body_noitom_impl, LiveFullBodyTrackerNoitomImpl::VENDOR_ID),
+    make_dispatch_entry<FullBodyTracker, LiveFullBodyTrackerXsensImpl>(
+        &try_create_full_body_xsens_impl, LiveFullBodyTrackerXsensImpl::VENDOR_ID),
     make_dispatch_entry<TensorPushTracker, LiveTensorPushTrackerImpl>(&try_create_tensor_push_impl),
     make_dispatch_entry<HapticCommandReaderTracker, LiveHapticCommandReaderTrackerImpl>(
         &try_create_haptic_command_reader_impl),
@@ -276,6 +285,10 @@ void validate_vendor_selections(const std::vector<std::pair<const ITracker*, Tra
         {
             LiveFullBodyTrackerNoitomImpl::validate_vendor(vendor);
         }
+        else if (vendor.id == LiveFullBodyTrackerXsensImpl::VENDOR_ID)
+        {
+            LiveFullBodyTrackerXsensImpl::validate_vendor(vendor);
+        }
         else if (!vendor.params.empty())
         {
             throw std::invalid_argument("LiveDeviceIOFactory: vendor params are not supported yet for tracker '" +
@@ -453,6 +466,19 @@ std::unique_ptr<IFullBodyTrackerImpl> LiveDeviceIOFactory::create_full_body_trac
     return std::make_unique<LiveFullBodyTrackerNoitomImpl>(handles_, *vendor, std::move(channels));
 }
 
+std::unique_ptr<IFullBodyTrackerImpl> LiveDeviceIOFactory::create_full_body_tracker_xsens_impl(const FullBodyTracker* tracker)
+{
+    std::unique_ptr<FullBodyMcapChannels> channels;
+    if (should_record(tracker))
+    {
+        channels = LiveFullBodyTrackerPicoImpl::create_mcap_channels(*writer_, get_name(tracker));
+    }
+
+    const TrackerVendor* vendor = find_vendor(tracker);
+    assert(vendor && vendor->id == LiveFullBodyTrackerXsensImpl::VENDOR_ID);
+    return std::make_unique<LiveFullBodyTrackerXsensImpl>(handles_, *vendor, std::move(channels));
+}
+
 std::unique_ptr<ITensorPushTrackerImpl> LiveDeviceIOFactory::create_tensor_push_tracker_impl(const TensorPushTracker* tracker)
 {
     return std::make_unique<LiveTensorPushTrackerImpl>(handles_, tracker);

src/core/live_trackers/cpp/live_full_body_tracker_xsens_impl.cpp

@@ -0,0 +1,97 @@
+// SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved.
+// SPDX-License-Identifier: Apache-2.0
+
+#include "live_full_body_tracker_xsens_impl.hpp"
+
+#include <charconv>
+#include <stdexcept>
+#include <string>
+#include <system_error>
+#include <utility>
+
+namespace core
+{
+
+namespace
+{
+
+constexpr std::string_view COLLECTION_ID_PARAM = "collection_id";
+constexpr std::string_view MAX_FLATBUFFER_SIZE_PARAM = "max_flatbuffer_size";
+
+SchemaTrackerConfig make_xsens_tensor_config(const TrackerVendor& vendor)
+{
+    if (vendor.id != LiveFullBodyTrackerXsensImpl::VENDOR_ID)
+    {
+        throw std::invalid_argument("Xsens full-body vendor id must be '" +
+                                    std::string(LiveFullBodyTrackerXsensImpl::VENDOR_ID) + "'");
+    }
+
+    for (const auto& [key, value] : vendor.params)
+    {
+        (void)value;
+        if (key != COLLECTION_ID_PARAM && key != MAX_FLATBUFFER_SIZE_PARAM)
+        {
+            throw std::invalid_argument("Xsens full-body vendor does not support parameter '" + key + "'");
+        }
+    }
+
+    SchemaTrackerConfig config;
+    config.collection_id = std::string(LiveFullBodyTrackerXsensImpl::DEFAULT_COLLECTION_ID);
+    config.max_flatbuffer_size = LiveFullBodyTrackerXsensImpl::DEFAULT_MAX_FLATBUFFER_SIZE;
+    config.tensor_identifier = std::string(LiveFullBodyTrackerXsensImpl::TENSOR_IDENTIFIER);
+    config.localized_name = "Xsens Full Body";
+
+    if (auto it = vendor.params.find(std::string(COLLECTION_ID_PARAM)); it != vendor.params.end())
+    {
+        // A collection_id mismatch between pusher and reader fails SILENTLY and forever -- the
+        // two sides rendezvous on this string and simply never connect. An empty one is always a
+        // configuration error, so reject it loudly here rather than hang later.
+        if (it->second.empty())
+        {
+            throw std::invalid_argument("Xsens full-body collection_id must not be empty");
+        }
+        config.collection_id = it->second;
+    }
+
+    if (auto it = vendor.params.find(std::string(MAX_FLATBUFFER_SIZE_PARAM)); it != vendor.params.end())
+    {
+        size_t parsed = 0;
+        const char* begin = it->second.data();
+        const char* end = begin + it->second.size();
+        const auto [ptr, error] = std::from_chars(begin, end, parsed);
+        if (error != std::errc{} || ptr != end || parsed == 0)
+        {
+            throw std::invalid_argument("Xsens full-body max_flatbuffer_size must be a positive integer");
+        }
+        config.max_flatbuffer_size = parsed;
+    }
+
+    return config;
+}
+
+} // namespace
+
+void LiveFullBodyTrackerXsensImpl::validate_vendor(const TrackerVendor& vendor)
+{
+    (void)make_xsens_tensor_config(vendor);
+}
+
+LiveFullBodyTrackerXsensImpl::LiveFullBodyTrackerXsensImpl(const OpenXRSessionHandles& handles,
+                                                           const TrackerVendor& vendor,
+                                                           std::unique_ptr<FullBodyXsensMcapChannels> mcap_channels)
+    : mcap_channels_(std::move(mcap_channels)),
+      schema_reader_(handles, make_xsens_tensor_config(vendor), mcap_channels_.get(), /*mcap_channel_index=*/0)
+{
+}
+
+void LiveFullBodyTrackerXsensImpl::update(int64_t /*monotonic_time_ns*/)
+{
+    schema_reader_.update(tracked_);
+}
+
+const Serialized<FullBodyPose>& LiveFullBodyTrackerXsensImpl::get_body_pose() const
+{
+    return tracked_;
+}
+
+} // namespace core

src/core/live_trackers/cpp/live_full_body_tracker_xsens_impl.hpp

@@ -0,0 +1,75 @@
+// SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved.
+// SPDX-License-Identifier: Apache-2.0
+
+#pragma once
+
+#include "inc/live_trackers/schema_tracker.hpp"
+
+#include <deviceio_base/full_body_tracker_base.hpp>
+#include <deviceio_base/tracker_vendor.hpp>
+#include <mcap/tracker_channels.hpp>
+#include <oxr_utils/oxr_session_handles.hpp>
+#include <schema/full_body_generated.h>
+
+#include <cstddef>
+#include <cstdint>
+#include <memory>
+#include <string_view>
+#include <vector>
+
+namespace core
+{
+
+using FullBodyXsensMcapChannels = McapTrackerChannels<FullBodyPoseRecord>;
+using FullBodyXsensSchemaTracker = SchemaTracker<FullBodyPoseRecord, FullBodyPose>;
+
+/*!
+ * @brief Full-body reader for the Xsens MVN stream pushed by the `xsens_full_body` plugin.
+ *
+ * Xsens MVN Studio converts its own 23-segment skeleton to the vendor-neutral 24-joint
+ * XR_BD_body_tracking layout inside MVN Studio itself (Options -> Network Streamer, preset
+ * "Isaac Teleop"), so what arrives here is already a `FullBodyPose` FlatBuffer -- byte-identical
+ * to what any other full-body vendor produces. This reader therefore adds nothing but the
+ * tensor-collection plumbing, exactly like the other pushed-collection full-body vendors.
+ *
+ * Two joints are always flagged invalid: MVN has no finger tracking, so LEFT_HAND (22) and
+ * RIGHT_HAND (23) carry a copy of the wrist pose with `is_valid = false`, and
+ * `all_joint_poses_tracked` is consequently always false. 22 of 24 valid is correct here, not a
+ * fault -- consult the per-joint flags.
+ */
+class LiveFullBodyTrackerXsensImpl : public IFullBodyTrackerImpl
+{
+public:
+    static constexpr std::string_view VENDOR_ID = "body.xsens";
+    static constexpr std::string_view DEFAULT_COLLECTION_ID = "xsens_full_body";
+    static constexpr std::string_view TENSOR_IDENTIFIER = "full_body_pose";
+    //! One pose is 16 B of table/vtable + 24 x 32 B of joint structs = 784 B. 4 KiB leaves room
+    //! without over-allocating; the pusher must be configured to match or both sides throw.
+    static constexpr size_t DEFAULT_MAX_FLATBUFFER_SIZE = 4 * 1024;
+
+    static std::vector<std::string> required_extensions()
+    {
+        return SchemaTrackerBase::get_required_extensions();
+    }
+
+    static void validate_vendor(const TrackerVendor& vendor);
+
+    LiveFullBodyTrackerXsensImpl(const OpenXRSessionHandles& handles,
+                                 const TrackerVendor& vendor,
+                                 std::unique_ptr<FullBodyXsensMcapChannels> mcap_channels);
+
+    LiveFullBodyTrackerXsensImpl(const LiveFullBodyTrackerXsensImpl&) = delete;
+    LiveFullBodyTrackerXsensImpl& operator=(const LiveFullBodyTrackerXsensImpl&) = delete;
+    LiveFullBodyTrackerXsensImpl(LiveFullBodyTrackerXsensImpl&&) = delete;
+    LiveFullBodyTrackerXsensImpl& operator=(LiveFullBodyTrackerXsensImpl&&) = delete;
+
+    void update(int64_t monotonic_time_ns) override;
+    const Serialized<FullBodyPose>& get_body_pose() const override;
+
+private:
+    std::unique_ptr<FullBodyXsensMcapChannels> mcap_channels_;
+    FullBodyXsensSchemaTracker schema_reader_;
+    Serialized<FullBodyPose> tracked_;
+};
+
+} // namespace core

src/plugins/xsens_full_body/CMakeLists.txt

@@ -0,0 +1,48 @@
+# SPDX-FileCopyrightText: Copyright (c) 2026 Xsens Technologies B.V. All rights reserved.
+# SPDX-License-Identifier: Apache-2.0
+
+if(NOT CMAKE_SYSTEM_NAME STREQUAL "Linux")
+    message(STATUS "Skipping xsens_full_body plugin (Linux only)")
+    add_custom_target(xsens_full_body_plugin
+        COMMAND ${CMAKE_COMMAND} -E echo "Skipping xsens_full_body: Linux only")
+    return()
+endif()
+
+# No vendor SDK: MVN Studio performs the skeleton conversion itself and emits the schema this
+# repo already defines, so the plugin needs nothing beyond the core libraries.
+add_executable(xsens_full_body_plugin
+    main.cpp
+    xsens_full_body_plugin.cpp
+    frame_decision.cpp
+    plugin_options.cpp
+)
+
+target_link_libraries(xsens_full_body_plugin PRIVATE
+    pusherio::pusherio
+    oxr::oxr_core
+    isaacteleop_schema
+)
+
+install(TARGETS xsens_full_body_plugin RUNTIME DESTINATION plugins/xsens_full_body)
+install(FILES plugin.yaml README.md DESTINATION plugins/xsens_full_body)
+
+# Unit tests for the two pieces the plugin can be taken apart into: what it decides about a
+# datagram, and what it decides about a command line. Both link their unit directly and need no
+# OpenXR runtime, so they run anywhere.
+if(BUILD_TESTING)
+    add_executable(test_xsens_frame_decision
+        tests/test_frame_decision.cpp
+        frame_decision.cpp
+    )
+    target_include_directories(test_xsens_frame_decision PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
+    target_link_libraries(test_xsens_frame_decision PRIVATE isaacteleop_schema)
+    add_test(NAME xsens_frame_decision COMMAND test_xsens_frame_decision)
+
+    # Lighter still than the above: argument parsing pulls in neither flatbuffers nor the schema.
+    add_executable(test_xsens_plugin_options
+        tests/test_plugin_options.cpp
+        plugin_options.cpp
+    )
+    target_include_directories(test_xsens_plugin_options PRIVATE ${CMAKE_CURRENT_SOURCE_DIR})
+    add_test(NAME xsens_plugin_options COMMAND test_xsens_plugin_options)
+endif()