diff --git a/docs/source/device/hand_tracking/manus.rst b/docs/source/device/hand_tracking/manus.rst index 237bff9c7e..7260f1369e 100644 --- a/docs/source/device/hand_tracking/manus.rst +++ b/docs/source/device/hand_tracking/manus.rst @@ -271,6 +271,25 @@ actively tracked wrist pose; otherwise it falls back to the controller pose. hand-tracking–to–controller switching to avoid unexpected source changes mid-session. +Controller offsets +~~~~~~~~~~~~~~~~~~ + +When the wrist comes from a controller, the plugin applies a fixed per-hand +offset from the controller's aim pose to the wrist. To use a different +controller or mount, override it in the environment that starts the plugin: + +.. code-block:: bash + + export MANUS_GLOVE_AIM_TO_WRIST_LEFT="px,py,pz,qx,qy,qz,qw" + export MANUS_GLOVE_AIM_TO_WRIST_RIGHT="px,py,pz,qx,qy,qz,qw" + +Each value is the wrist pose in the controller's aim frame: position in +meters, then an ``(x, y, z, w)`` quaternion. For mirror-image mounts, the left +value is the right one with ``px``, ``qy`` and ``qz`` negated. A plugin started +by ``TeleopSession`` inherits these variables. The plugin logs each override it +applies; a value it cannot use is reported as a warning and the built-in offset +stays in effect. + Troubleshooting --------------- diff --git a/src/plugins/manus/core/inc/manus/manus_hand_tracking_plugin.hpp b/src/plugins/manus/core/inc/manus/manus_hand_tracking_plugin.hpp index 8370414af8..8d29c841f9 100644 --- a/src/plugins/manus/core/inc/manus/manus_hand_tracking_plugin.hpp +++ b/src/plugins/manus/core/inc/manus/manus_hand_tracking_plugin.hpp @@ -157,6 +157,10 @@ class __attribute__((visibility("default"))) ManusTracker std::unique_ptr m_left_injector; std::unique_ptr m_right_injector; std::shared_ptr m_controller_tracker; + // Controller aim pose -> wrist joint per hand; set in initialize() from the + // built-in offsets or their environment override. + XrPosef m_left_aim_to_wrist = { { 0.0f, 0.0f, 0.0f, 1.0f }, { 0.0f, 0.0f, 0.0f } }; + XrPosef m_right_aim_to_wrist = { { 0.0f, 0.0f, 0.0f, 1.0f }, { 0.0f, 0.0f, 0.0f } }; std::shared_ptr m_hand_tracker; // Inbound HapticCommand tensor; collection identity in // inc/manus/manus_glove_collection.hpp. Read each frame in update(). diff --git a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp index 9daa3f9984..b4e084a4c3 100644 --- a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp +++ b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp @@ -38,6 +38,8 @@ #include #include #include +#include +#include #include #include #include @@ -123,8 +125,65 @@ std::vector read_calibration_file(const std::string& path) return data; } +// MANUS_GLOVE_AIM_TO_WRIST_{LEFT,RIGHT}="px,py,pz,qx,qy,qz,qw" (meters, then quaternion) replaces that hand's +// controller-to-wrist offset +XrPosef aim_to_wrist_from_env(const char* name, const XrPosef& fallback, spdlog::logger& logger) +{ + const char* value = std::getenv(name); + if (value == nullptr || *value == '\0') + { + return fallback; + } + + XrPosef pose{}; + int consumed = -1; + // The spaces accept whitespace around each comma; %n then proves that only + // whitespace follows the seventh value. + const int parsed = + std::sscanf(value, "%f ,%f ,%f ,%f ,%f ,%f ,%f %n", &pose.position.x, &pose.position.y, &pose.position.z, + &pose.orientation.x, &pose.orientation.y, &pose.orientation.z, &pose.orientation.w, &consumed); + if (parsed != 7 || consumed < 0 || value[consumed] != '\0') + { + logger.warn("Ignoring {}='{}': expected px,py,pz,qx,qy,qz,qw; using the built-in offset", name, value); + return fallback; + } + + const std::array components{ pose.position.x, pose.position.y, pose.position.z, + pose.orientation.x, pose.orientation.y, pose.orientation.z, + pose.orientation.w }; + if (!std::all_of(components.begin(), components.end(), [](float c) { return std::isfinite(c); })) + { + logger.warn("Ignoring {}='{}': values must be finite; using the built-in offset", name, value); + return fallback; + } + + XrQuaternionf& q = pose.orientation; + const double norm = std::sqrt(static_cast(q.x) * q.x + static_cast(q.y) * q.y + + static_cast(q.z) * q.z + static_cast(q.w) * q.w); + if (norm < 1e-6f) + { + logger.warn("Ignoring {}='{}': the quaternion is zero; using the built-in offset", name, value); + return fallback; + } + // Rounding keeps a unit quaternion's norm well inside this band; a value + // written in XrPosef order (quaternion first) usually lands outside it. + if (std::abs(norm - 1.0f) > 0.05f) + { + logger.warn("{}: quaternion norm is {:.3f}, normalizing it; check the order is px,py,pz,qx,qy,qz,qw", name, norm); + } + q.x /= norm; + q.y /= norm; + q.z /= norm; + q.w /= norm; + + logger.info("{} overrides the built-in controller-to-wrist offset: {},{},{},{},{},{},{}", name, pose.position.x, + pose.position.y, pose.position.z, q.x, q.y, q.z, q.w); + return pose; +} + } // anonymous namespace +// Controller aim pose -> wrist joint, used unless aim_to_wrist_from_env() finds an override. static constexpr XrPosef kLeftHandOffset = { { -0.744f, -0.445f, -0.282f, 0.411f }, { -0.072f, -0.001f, -0.003f } }; static constexpr XrPosef kRightHandOffset = { { -0.744f, 0.445f, 0.282f, 0.411f }, { 0.072f, -0.001f, -0.003f } }; @@ -358,6 +417,9 @@ void ManusTracker::initialize() noexcept(false) if (m_config.human) { + m_left_aim_to_wrist = aim_to_wrist_from_env("MANUS_GLOVE_AIM_TO_WRIST_LEFT", kLeftHandOffset, *m_logger); + m_right_aim_to_wrist = aim_to_wrist_from_env("MANUS_GLOVE_AIM_TO_WRIST_RIGHT", kRightHandOffset, *m_logger); + // Create ControllerTracker unconditionally; HandTracker requires // XR_EXT_hand_tracking which is optional — only add it when the runtime // advertises support so xrCreateInstance does not fail with @@ -1263,7 +1325,7 @@ bool ManusTracker::get_controller_wrist_pose(bool is_left, XrPosef& out_wrist_po return false; } - XrPosef offset_pose = is_left ? kLeftHandOffset : kRightHandOffset; + const XrPosef& offset_pose = is_left ? m_left_aim_to_wrist : m_right_aim_to_wrist; out_wrist_pose = oxr_utils::multiply_poses(raw_pose, offset_pose); return true; }