From f422e2045b21248dcbed30b9acbdb6c24ee7d0fb Mon Sep 17 00:00:00 2001 From: zerenluo Date: Tue, 4 Aug 2026 17:23:33 +0800 Subject: [PATCH] [Add] Meta IOBT body tracking: expose BodyMeta as get_body_meta_* API --- bindings/py_bindings.cpp | 183 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 183 insertions(+) diff --git a/bindings/py_bindings.cpp b/bindings/py_bindings.cpp index 4f7c9d9..903ecb6 100644 --- a/bindings/py_bindings.cpp +++ b/bindings/py_bindings.cpp @@ -6,11 +6,14 @@ #include #include #include +#include +#include #include #include "PXREARobotSDK.h" using json = nlohmann::json; +namespace py = pybind11; std::array LeftControllerPose; std::array RightControllerPose; @@ -31,6 +34,25 @@ std::array BodyJointsTimestamp; // IMU timestamp for each joint int64_t BodyTimeStampNs = 0; // Body data timestamp bool BodyDataAvailable = false; // Flag to indicate if body data is available +// Meta IOBT body tracking ("BodyMeta"), kept entirely separate from the PICO "Body" path above. +// Two reasons it cannot reuse it: the arrays there are fixed at 24 and would silently drop 46 of +// the 70 UpperBody joints, and Meta reports no velocity/acceleration at all (BodyJointLocation +// carries only LocationFlags and Pose, unlike PICO's IMU trackers). +// +// std::vector rather than std::array: the joint count is whatever the client sends, so adding +// joints upstream -- or switching to the 84-joint FullBody skeleton -- needs no change here. +std::vector BodyMetaJointIds; // Meta joint id, see get_body_meta_joint_ids +std::vector> BodyMetaJointsPose; // x,y,z,qx,qy,qz,qw per joint +std::vector BodyMetaPositionValid; // per joint, false when occluded +std::vector BodyMetaOrientationValid; // per joint, fails independently of above +std::array BodyMetaTrackingSpace{0, 0, 0, 0, 0, 0, 1}; // tracking space in world frame +std::string BodyMetaJointSet; // "UpperBody" / "FullBody" +std::string BodyMetaFidelity; // "High" = camera-measured IOBT +std::string BodyMetaCalibration; // "Valid" once skeleton scale settles +double BodyMetaConfidence = 0.0; +int64_t BodyMetaTimeStampNs = 0; +bool BodyMetaDataAvailable = false; + std::array, 3> MotionTrackerPose; // Position and rotation for each joint std::array, 3> MotionTrackerVelocity; // Velocity and angular velocity for each joint std::array, 3> MotionTrackerAcceleration; // Acceleration and angular acceleration for each joint @@ -64,6 +86,7 @@ std::mutex timestampMutex; std::mutex leftHandMutex; std::mutex rightHandMutex; std::mutex bodyMutex; // Mutex for body tracking data +std::mutex bodyMetaMutex; // Mutex for Meta IOBT body tracking data std::mutex motionMutex; @@ -223,6 +246,79 @@ void OnPXREAClientCallback(void* context, PXREAClientCallbackType type, int stat } } } + // Parse BodyMeta: Meta IOBT upper-body joints from the Quest client. Independent + // of the "Body" branch above -- neither reads the other's key or storage. + if (value.contains("BodyMeta")) { + auto& bodyMeta = value["BodyMeta"]; + { + std::lock_guard lock(bodyMetaMutex); + + // Unlike PICO, the timestamp is at the top level of the packet, not inside + // the body object: all sources in a frame share one, which is what makes + // headset pose and joints alignable. + if (value.contains("timeStampNs")) { + BodyMetaTimeStampNs = value["timeStampNs"].get(); + } + + // The headset's mount detection gates tracking, so once it leaves the head + // (neck included) isActive goes 0 while "joints" still holds the last frame + // -- the client reuses its JSON objects and does not clear them. Reporting + // unavailable is the whole point of the flag; a stale frame read as live is + // worse than no frame, because nothing downstream can tell it is stale. + const bool isActive = bodyMeta.contains("isActive") && + bodyMeta["isActive"].get() != 0; + if (!isActive) { + BodyMetaDataAvailable = false; + } else if (bodyMeta.contains("joints") && bodyMeta["joints"].is_array()) { + if (bodyMeta.contains("jointSet")) { + BodyMetaJointSet = bodyMeta["jointSet"].get(); + } + if (bodyMeta.contains("fidelity")) { + BodyMetaFidelity = bodyMeta["fidelity"].get(); + } + if (bodyMeta.contains("calib")) { + BodyMetaCalibration = bodyMeta["calib"].get(); + } + if (bodyMeta.contains("confidence")) { + BodyMetaConfidence = bodyMeta["confidence"].get(); + } + if (bodyMeta.contains("trackingSpace")) { + BodyMetaTrackingSpace = stringToPoseArray( + bodyMeta["trackingSpace"].get()); + } + + auto& joints = bodyMeta["joints"]; + const size_t jointCount = joints.size(); + + // Resized, never truncated: the count is data, not a constant. + BodyMetaJointIds.resize(jointCount); + BodyMetaJointsPose.resize(jointCount); + BodyMetaPositionValid.resize(jointCount); + BodyMetaOrientationValid.resize(jointCount); + + for (size_t i = 0; i < jointCount; i++) { + auto& joint = joints[i]; + + BodyMetaJointIds[i] = joint.contains("id") + ? joint["id"].get() : static_cast(i); + + if (joint.contains("p")) { + BodyMetaJointsPose[i] = + stringToPoseArray(joint["p"].get()); + } + + // Occluded joints keep their last pose, so validity has to travel + // with the data for consumers to know what to trust. + BodyMetaPositionValid[i] = + joint.contains("v") && joint["v"].get() != 0; + BodyMetaOrientationValid[i] = + joint.contains("vr") && joint["vr"].get() != 0; + } + + BodyMetaDataAvailable = true; + } + } + } //parse individual tracker data if (value.contains("Motion")) { auto& motion = value["Motion"]; @@ -431,6 +527,62 @@ int64_t getBodyTimeStampNs() { return BodyTimeStampNs; } +// Meta IOBT body tracking functions. Named get_body_meta_* alongside the get_body_* above: the +// two are parallel, independent sources, and neither name is taken from the other. +// +// There is deliberately no get_body_meta_joints_velocity/_acceleration/_timestamp counterpart: +// Meta's BodyJointLocation has no such fields, and returning zeros under a familiar name would be +// worse than the function not existing. +bool isBodyMetaAvailable() { + std::lock_guard lock(bodyMetaMutex); + return BodyMetaDataAvailable; +} + +std::vector> getBodyMetaJointsPose() { + std::lock_guard lock(bodyMetaMutex); + return BodyMetaJointsPose; +} + +std::vector getBodyMetaJointIds() { + std::lock_guard lock(bodyMetaMutex); + return BodyMetaJointIds; +} + +std::vector getBodyMetaPositionValid() { + std::lock_guard lock(bodyMetaMutex); + return BodyMetaPositionValid; +} + +std::vector getBodyMetaOrientationValid() { + std::lock_guard lock(bodyMetaMutex); + return BodyMetaOrientationValid; +} + +std::array getBodyMetaTrackingSpace() { + std::lock_guard lock(bodyMetaMutex); + return BodyMetaTrackingSpace; +} + +int64_t getBodyMetaTimeStampNs() { + std::lock_guard lock(bodyMetaMutex); + return BodyMetaTimeStampNs; +} + +// Grouped into one dict rather than six getters: these are read together when deciding whether a +// frame is trustworthy, and each would otherwise take its own lock. +py::dict getBodyMetaInfo() { + std::lock_guard lock(bodyMetaMutex); + py::dict info; + info["available"] = BodyMetaDataAvailable; + info["count"] = BodyMetaJointsPose.size(); + info["jointSet"] = BodyMetaJointSet; + info["fidelity"] = BodyMetaFidelity; + info["calib"] = BodyMetaCalibration; + info["confidence"] = BodyMetaConfidence; + info["timeStampNs"] = BodyMetaTimeStampNs; + return info; +} + int numMotionDataAvailable() { std::lock_guard lock(motionMutex); return NumMotionDataAvailable; @@ -529,6 +681,37 @@ PYBIND11_MODULE(xrobotoolkit_sdk, m) { m.def("get_body_joints_timestamp", &getBodyJointsTimestamp, "Get the body joints IMU timestamp data (24 joints)."); m.def("get_body_timestamp_ns", &getBodyTimeStampNs, "Get the body data timestamp in nanoseconds."); + // Meta IOBT body tracking (Quest). Parallel to the get_body_* group above and independent of + // it: these read the "BodyMeta" key, those read "Body". Joint count is not fixed (70 for the + // UpperBody skeleton) and every joint is camera-measured. + m.def("is_body_meta_available", &isBodyMetaAvailable, + "Check if Meta IOBT body data is available. Call this before reading anything else: it " + "goes False whenever the headset leaves the head, neck included, and the joint arrays " + "then hold stale values that look perfectly valid."); + m.def("get_body_meta_joints_pose", &getBodyMetaJointsPose, + "Get Meta IOBT joint poses, (N, 7): x,y,z,qx,qy,qz,qw. Coordinates are TRACKING-SPACE " + "local, so they describe posture only -- combine with get_body_meta_tracking_space() to " + "recover where the operator is standing."); + m.def("get_body_meta_joint_ids", &getBodyMetaJointIds, + "Get Meta IOBT joint ids, (N,). Index into Meta's BodyJointId enum; read together with " + "get_body_meta_info()['jointSet'], since UpperBody and FullBody number joints " + "differently."); + m.def("get_body_meta_position_valid", &getBodyMetaPositionValid, + "Get per-joint position validity, (N,). False under occlusion, where the joint keeps its " + "last position."); + m.def("get_body_meta_orientation_valid", &getBodyMetaOrientationValid, + "Get per-joint orientation validity, (N,). Fails independently of position validity."); + m.def("get_body_meta_tracking_space", &getBodyMetaTrackingSpace, + "Get the tracking space pose in world coordinates, (7,). Required to reconstruct root " + "locomotion: joint coordinates are local to this space."); + m.def("get_body_meta_timestamp_ns", &getBodyMetaTimeStampNs, + "Get the Meta IOBT data timestamp in nanoseconds. Shared with all other sources in the " + "same packet, so poses across sources are directly comparable."); + m.def("get_body_meta_info", &getBodyMetaInfo, + "Get data-quality metadata: available, count, jointSet, fidelity, calib, confidence, " + "timeStampNs. Gate on calib == 'Valid' (skeleton scale is still settling before that) " + "and fidelity == 'High' (Low is IK inference, not camera-measured)."); + // Motion tracker functions m.def("num_motion_data_available", &numMotionDataAvailable, "Check if motion tracker data is available."); m.def("get_motion_tracker_pose", &getMotionTrackerPose, "Get the motion tracker pose data (3 trackers, 7 values each: x,y,z,qx,qy,qz,qw).");