+1

doc/changes/drivers/mr.2425.md

··· 1 1 + - steamvr_lh: generate hand tracking and palm pose from skeleton

+351 -96

src/xrt/drivers/steamvr_lh/device.cpp

··· 9 9 */ 10 10 11 11 #include "math/m_api.h" 12 12 + #include "math/m_predict.h" 12 13 #include "math/m_relation_history.h" 13 14 #include "math/m_space.h" 14 15 ··· 16 17 17 18 #include "util/u_debug.h" 18 19 #include "util/u_device.h" 19 19 - #include "util/u_hand_simulation.h" 20 20 #include "util/u_hand_tracking.h" 21 21 #include "util/u_logging.h" 22 22 #include "util/u_json.hpp" 23 23 24 24 + #include "util/u_time.h" 24 25 #include "xrt/xrt_defines.h" 25 26 #include "xrt/xrt_device.h" 26 27 #include "xrt/xrt_prober.h" ··· 34 35 #include <cmath> 35 36 #include <functional> 36 37 #include <cstring> 38 38 + #include <numbers> 39 39 + #include <openvr_driver.h> 37 40 #include <thread> 38 41 #include <algorithm> 39 42 #include <map> 40 40 - 41 43 42 44 #define DEV_ERR(...) U_LOG_IFL_E(ctx->log_level, __VA_ARGS__) 43 45 #define DEV_WARN(...) U_LOG_IFL_W(ctx->log_level, __VA_ARGS__) ··· 52 54 xrt_device_name name; 53 55 const std::vector<xrt_input_name> poses; 54 56 const std::unordered_map<std::string_view, xrt_input_name> non_poses; 55 55 - const std::unordered_map<std::string_view, IndexFinger> finger_curls; 56 57 }; 57 58 58 59 namespace { 60 60 + using namespace std::string_view_literals; 61 61 + // From https://github.com/ValveSoftware/openvr/blob/master/docs/Driver_API_Documentation.md#bone-structure 62 62 + enum HandSkeletonBone : int32_t 63 63 + { 64 64 + eBone_Root = 0, 65 65 + eBone_Wrist, 66 66 + eBone_Thumb0, 67 67 + eBone_Thumb1, 68 68 + eBone_Thumb2, 69 69 + eBone_Thumb3, 70 70 + eBone_IndexFinger0, 71 71 + eBone_IndexFinger1, 72 72 + eBone_IndexFinger2, 73 73 + eBone_IndexFinger3, 74 74 + eBone_IndexFinger4, 75 75 + eBone_MiddleFinger0, 76 76 + eBone_MiddleFinger1, 77 77 + eBone_MiddleFinger2, 78 78 + eBone_MiddleFinger3, 79 79 + eBone_MiddleFinger4, 80 80 + eBone_RingFinger0, 81 81 + eBone_RingFinger1, 82 82 + eBone_RingFinger2, 83 83 + eBone_RingFinger3, 84 84 + eBone_RingFinger4, 85 85 + eBone_PinkyFinger0, 86 86 + eBone_PinkyFinger1, 87 87 + eBone_PinkyFinger2, 88 88 + eBone_PinkyFinger3, 89 89 + eBone_PinkyFinger4, 90 90 + eBone_Aux_Thumb, 91 91 + eBone_Aux_IndexFinger, 92 92 + eBone_Aux_MiddleFinger, 93 93 + eBone_Aux_RingFinger, 94 94 + eBone_Aux_PinkyFinger, 95 95 + eBone_Count 96 96 + }; 97 97 + 59 98 // Adding support for a new controller is a simple as adding it here. 60 99 // The key for the map needs to be the name of input profile as indicated by the lighthouse driver. 61 100 const std::unordered_map<std::string_view, InputClass> controller_classes{ ··· 66 105 { 67 106 XRT_INPUT_VIVE_GRIP_POSE, 68 107 XRT_INPUT_VIVE_AIM_POSE, 108 108 + XRT_INPUT_GENERIC_PALM_POSE, 69 109 }, 70 110 { 71 111 {"/input/application_menu/click", XRT_INPUT_VIVE_MENU_CLICK}, ··· 76 116 {"/input/trigger/value", XRT_INPUT_VIVE_TRIGGER_VALUE}, 77 117 {"/input/grip/click", XRT_INPUT_VIVE_SQUEEZE_CLICK}, 78 118 {"/input/trackpad", XRT_INPUT_VIVE_TRACKPAD}, 79 79 - }, 80 80 - { 81 81 - // No fingers on this controller type 82 119 }, 83 120 }, 84 121 }, ··· 89 126 { 90 127 XRT_INPUT_INDEX_GRIP_POSE, 91 128 XRT_INPUT_INDEX_AIM_POSE, 129 129 + XRT_INPUT_GENERIC_PALM_POSE, 92 130 }, 93 131 { 94 132 {"/input/system/click", XRT_INPUT_INDEX_SYSTEM_CLICK}, ··· 108 146 {"/input/trackpad/force", XRT_INPUT_INDEX_TRACKPAD_FORCE}, 109 147 {"/input/trackpad/touch", XRT_INPUT_INDEX_TRACKPAD_TOUCH}, 110 148 {"/input/trackpad", XRT_INPUT_INDEX_TRACKPAD}, 111 111 - }, 112 112 - { 113 113 - {"/input/finger/index", IndexFinger::Index}, 114 114 - {"/input/finger/middle", IndexFinger::Middle}, 115 115 - {"/input/finger/ring", IndexFinger::Ring}, 116 116 - {"/input/finger/pinky", IndexFinger::Pinky}, 117 149 }, 118 150 }, 119 151 }, ··· 131 163 {"/input/trigger/click", XRT_INPUT_VIVE_TRACKER_TRIGGER_CLICK}, 132 164 {"/input/thumb/click", XRT_INPUT_VIVE_TRACKER_TRACKPAD_CLICK}, 133 165 }, 134 134 - { 135 135 - // No fingers on this controller type 136 136 - }, 137 166 }, 138 167 }, 139 168 { ··· 150 179 {"/input/trigger/click", XRT_INPUT_VIVE_TRACKER_TRIGGER_CLICK}, 151 180 {"/input/thumb/click", XRT_INPUT_VIVE_TRACKER_TRACKPAD_CLICK}, 152 181 }, 153 153 - { 154 154 - // No fingers on this controller type 155 155 - }, 156 182 }, 157 183 }, 158 184 }; 159 159 - 160 185 int64_t 161 186 chrono_timestamp_ns() 162 187 { ··· 214 239 215 240 return brightness; 216 241 } 242 242 + xrt_pose 243 243 + bone_to_pose(const vr::VRBoneTransform_t &bone) 244 244 + { 245 245 + return xrt_pose{xrt_quat{bone.orientation.x, bone.orientation.y, bone.orientation.z, bone.orientation.w}, 246 246 + xrt_vec3{bone.position.v[0], bone.position.v[1], bone.position.v[2]}}; 247 247 + } 217 248 } // namespace 218 249 219 250 Property::Property(vr::PropertyTypeTag_t tag, void *buffer, uint32_t bufferSize) ··· 249 280 this->xrt_device::get_hand_tracking = 250 281 &device_bouncer<ControllerDevice, &ControllerDevice::get_hand_tracking, xrt_result_t>; 251 282 this->xrt_device::set_output = &device_bouncer<ControllerDevice, &ControllerDevice::set_output, xrt_result_t>; 283 283 + 284 284 + this->inputs_map["/skeleton/hand/left"] = &hand_tracking_inputs[XRT_HAND_LEFT]; 285 285 + this->inputs_map["/skeleton/hand/right"] = &hand_tracking_inputs[XRT_HAND_RIGHT]; 252 286 } 253 287 254 288 Device::~Device() ··· 285 319 init_chaperone(builder.steam_install); 286 320 } 287 321 288 288 - void 289 289 - ControllerDevice::set_hand_tracking_hand(xrt_input_name name) 290 290 - { 291 291 - if (has_index_hand_tracking) { 292 292 - inputs_map["HAND"]->name = name; 293 293 - } 294 294 - } 295 295 - 296 322 // NOTE: No operations that would force inputs_vec or finger_inputs_vec to reallocate (such as insertion) 297 323 // should be done after this function is called, otherwise the pointers in inputs_map/finger_inputs_map 298 324 // would be invalidated. ··· 314 340 inputs_map.insert({path, &inputs_vec.back()}); 315 341 } 316 342 317 317 - has_index_hand_tracking = debug_get_bool_option_lh_emulate_hand() && !input_class->finger_curls.empty(); 318 318 - if (has_index_hand_tracking) { 319 319 - finger_inputs_vec.reserve(input_class->finger_curls.size()); 320 320 - for (const auto &[path, finger] : input_class->finger_curls) { 321 321 - assert(finger_inputs_vec.capacity() >= finger_inputs_vec.size() + 1); 322 322 - finger_inputs_vec.push_back({0, finger, 0.f}); 323 323 - finger_inputs_map.insert({path, &finger_inputs_vec.back()}); 324 324 - } 325 325 - assert(inputs_vec.capacity() >= inputs_vec.size() + 1); 326 326 - inputs_vec.push_back({true, 0, XRT_INPUT_HT_CONFORMING_LEFT, {}}); 327 327 - inputs_map.insert({std::string_view("HAND"), &inputs_vec.back()}); 328 328 - } 329 329 - 330 343 this->inputs = inputs_vec.data(); 331 344 this->input_count = inputs_vec.size(); 332 345 } ··· 345 358 } 346 359 } 347 360 348 348 - const std::vector<std::string> FACE_BUTTONS = { 349 349 - "/input/system/touch", "/input/a/touch", "/input/b/touch", "/input/thumbstick/touch", "/input/trackpad/touch", 350 350 - }; 361 361 + void 362 362 + ControllerDevice::set_active_hand(xrt_hand hand) 363 363 + { 364 364 + this->skeleton_hand = hand; 365 365 + } 366 366 + 367 367 + namespace { 368 368 + xrt_quat 369 369 + from_euler_angles(float x, float y, float z) 370 370 + { 371 371 + const xrt_vec3 v{x, y, z}; 372 372 + xrt_quat out; 373 373 + math_quat_from_euler_angles(&v, &out); 374 374 + return out; 375 375 + } 376 376 + 377 377 + constexpr float pi = std::numbers::pi_v<float>; 378 378 + constexpr float frac_pi_2 = pi / 2.0f; 379 379 + 380 380 + // OpenVR skeletal poses are defined with the palms facing each other, but OpenXR 381 381 + // hand tracking is defined with the palms facing down. These per hand rotations 382 382 + // are necessary to translate to what OpenXR expects. 383 383 + const xrt_quat right_hand_rotate = from_euler_angles(0.0f, frac_pi_2, 0.0f); 384 384 + const xrt_quat left_hand_rotate = from_euler_angles(0.0f, frac_pi_2, pi); 385 385 + 386 386 + xrt_quat 387 387 + right_wrist_rotate_init() 388 388 + { 389 389 + const xrt_quat rot1 = from_euler_angles(0.0f, 0.0f, frac_pi_2); 390 390 + const xrt_quat rot2 = from_euler_angles(0.0f, pi, 0.0f); 391 391 + xrt_quat ret; 392 392 + math_quat_rotate(&rot1, &rot2, &ret); 393 393 + return ret; 394 394 + } 395 395 + xrt_quat 396 396 + left_wrist_rotate_init() 397 397 + { 398 398 + const xrt_quat rot1 = from_euler_angles(pi, 0.0f, 0.0f); 399 399 + const xrt_quat rot2 = from_euler_angles(0.0f, 0.0f, -frac_pi_2); 400 400 + xrt_quat ret; 401 401 + math_quat_rotate(&rot1, &rot2, &ret); 402 402 + return ret; 403 403 + } 404 404 + 405 405 + const xrt_quat right_wrist_rotate = right_wrist_rotate_init(); 406 406 + const xrt_quat left_wrist_rotate = left_wrist_rotate_init(); 407 407 + 408 408 + xrt_pose 409 409 + generate_palm_pose(const xrt_pose &metacarpal_pose, const xrt_pose &proximal_pose) 410 410 + { 411 411 + // OpenVR doesn't provide a palm joint, but the OpenXR palm is in the middle of 412 412 + // the metacarpal and proximal bones of the middle finger, 413 413 + // so we'll interpolate between them to generate it. 414 414 + xrt_pose pose; 415 415 + math_pose_interpolate(&metacarpal_pose, &proximal_pose, 0.5, &pose); 416 416 + // Use metacarpal orientation, because the palm shouldn't really rotate 417 417 + pose.orientation = metacarpal_pose.orientation; 418 418 + return pose; 419 419 + } 420 420 + 421 421 + xrt_pose 422 422 + palm_offset_index(xrt_hand hand) 423 423 + { 424 424 + // Taken from: 425 425 + // https://github.com/ValveSoftware/OpenXR-Canonical-Pose-Tool/blob/5e6f3f6db584d58483058ff3262e7eef02c3acfd/dist/steamvr/cpt_SteamVR-valve_index_controller.xml#L1 426 426 + switch (hand) { 427 427 + case XRT_HAND_LEFT: 428 428 + return xrt_pose{.orientation = xrt_quat{.x = -0.46, .y = -0.02, .z = -0.01, .w = 0.89}, 429 429 + .position = xrt_vec3{.x = -0.015, .y = 0.0, .z = 0.001}}; 430 430 + case XRT_HAND_RIGHT: 431 431 + return xrt_pose{.orientation = xrt_quat{.x = -0.46, .y = 0.02, .z = 0.01, .w = 0.89}, 432 432 + .position = xrt_vec3{.x = 0.015, .y = 0.0, .z = 0.001}}; 433 433 + } 434 434 + 435 435 + return {}; 436 436 + } 437 437 + 438 438 + } // namespace 439 439 + 440 440 + void 441 441 + ControllerDevice::set_skeleton(std::span<const vr::VRBoneTransform_t> bones, 442 442 + xrt_hand hand, 443 443 + bool is_simulated, 444 444 + const char *path) 445 445 + { 446 446 + assert(bones.size() == eBone_Count); 447 447 + generate_palm_pose_offset(bones, hand); 448 448 + if (!is_simulated && debug_get_bool_option_lh_emulate_hand()) { 449 449 + assert(inputs_vec.capacity() >= inputs_vec.size() + 1); 450 450 + const xrt_input_name tracker_name = 451 451 + (hand == XRT_HAND_RIGHT) ? XRT_INPUT_HT_CONFORMING_RIGHT : XRT_INPUT_HT_CONFORMING_LEFT; 452 452 + inputs_vec.push_back({true, 0, tracker_name, {}}); 453 453 + inputs_map.insert({path, &inputs_vec.back()}); 454 454 + this->input_count = inputs_vec.size(); 455 455 + has_hand_tracking = true; 456 456 + } 457 457 + } 458 458 + 459 459 + void 460 460 + ControllerDevice::generate_palm_pose_offset(std::span<const vr::VRBoneTransform_t> bones, xrt_hand hand) 461 461 + { 462 462 + if (this->input_class->name == XRT_DEVICE_INDEX_CONTROLLER) { 463 463 + xrt_pose grip_offset; 464 464 + vive_poses_get_pose_offset(this->input_class->name, this->device_type, XRT_INPUT_INDEX_GRIP_POSE, 465 465 + &grip_offset); 466 466 + auto offset = palm_offset_index(hand); 467 467 + math_pose_transform(&grip_offset, &offset, &offset); 468 468 + palm_offsets[hand] = offset; 469 469 + return; 470 470 + } 471 471 + // The palm pose offset is generated from the OpenVR provided skeleton. 472 472 + // https://github.com/ValveSoftware/openvr/blob/master/docs/Driver_API_Documentation.md#notes-on-the-skeleton 473 473 + 474 474 + xrt_pose root = bone_to_pose(bones[eBone_Root]); 475 475 + xrt_pose wrist = bone_to_pose(bones[eBone_Wrist]); 476 476 + xrt_pose metacarpal = bone_to_pose(bones[eBone_MiddleFinger0]); 477 477 + xrt_pose proximal = bone_to_pose(bones[eBone_MiddleFinger1]); 478 478 + 479 479 + // The skeleton pose is given with the Root bone as origin. 480 480 + // To convert from this, according to OpenVR docs we transform the wrist 481 481 + // and then counter-transform the metacarpals 482 482 + xrt_pose root_inv; 483 483 + math_pose_invert(&root, &root_inv); 484 484 + math_pose_transform(&root_inv, &wrist, &wrist); 485 485 + math_pose_transform(&root, &metacarpal, &metacarpal); 486 486 + math_pose_transform(&wrist, &metacarpal, &metacarpal); 487 487 + math_pose_transform(&metacarpal, &proximal, &proximal); 488 488 + 489 489 + xrt_pose palm_offset = generate_palm_pose(metacarpal, proximal); 490 490 + xrt_quat palm_rotate = from_euler_angles(0.0f, 0.0f, frac_pi_2); 491 491 + 492 492 + switch (hand) { 493 493 + case XRT_HAND_LEFT: { 494 494 + math_quat_rotate(&palm_offset.orientation, &left_hand_rotate, &palm_offset.orientation); 495 495 + math_quat_invert(&palm_rotate, &palm_rotate); 496 496 + break; 497 497 + } 498 498 + case XRT_HAND_RIGHT: { 499 499 + math_quat_rotate(&palm_offset.orientation, &right_hand_rotate, &palm_offset.orientation); 500 500 + break; 501 501 + } 502 502 + } 503 503 + math_quat_rotate(&palm_offset.orientation, &palm_rotate, &palm_offset.orientation); 504 504 + 505 505 + // For controllers like the Vive Wands which can be in any hand, it will store both the left hand 506 506 + // and the right hand skeletons, so we need to store both. 507 507 + palm_offsets[hand] = palm_offset; 508 508 + } 351 509 352 510 void 353 353 - ControllerDevice::update_hand_tracking(int64_t desired_timestamp_ns, struct xrt_hand_joint_set *out) 511 511 + ControllerDevice::update_skeleton_transforms(std::span<const vr::VRBoneTransform_t> bones) 354 512 { 355 355 - if (!has_index_hand_tracking) 513 513 + if (!has_hand_tracking) { 514 514 + return; 515 515 + } 516 516 + 517 517 + assert(bones.size() == eBone_Count); 518 518 + 519 519 + xrt_hand_joint_set joint_set; 520 520 + int64_t timestamp; 521 521 + if (!m_relation_history_get_latest(relation_hist, &timestamp, &joint_set.hand_pose)) { 356 522 return; 357 357 - float index = 0.f; 358 358 - float middle = 0.f; 359 359 - float ring = 0.f; 360 360 - float pinky = 0.f; 361 361 - float thumb = 0.f; 362 362 - for (auto fi : finger_inputs_vec) { 363 363 - switch (fi.finger) { 364 364 - case IndexFinger::Index: index = fi.value; break; 365 365 - case IndexFinger::Middle: middle = fi.value; break; 366 366 - case IndexFinger::Ring: ring = fi.value; break; 367 367 - case IndexFinger::Pinky: pinky = fi.value; break; 368 368 - default: break; 369 369 - } 523 523 + } 524 524 + joint_set.is_active = true; 525 525 + auto &joints = joint_set.values.hand_joint_set_default; 526 526 + 527 527 + xrt_pose root = bone_to_pose(bones[eBone_Root]); 528 528 + xrt_pose wrist = bone_to_pose(bones[eBone_Wrist]); 529 529 + 530 530 + // Here we're doing the same transformation as seen in generate_palm_pose_offset. 531 531 + xrt_pose root_inv; 532 532 + math_pose_invert(&root, &root_inv); 533 533 + math_pose_transform(&root_inv, &wrist, &wrist); 534 534 + 535 535 + constexpr auto valid_flags = (enum xrt_space_relation_flags)( 536 536 + XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | 537 537 + XRT_SPACE_RELATION_POSITION_TRACKED_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); 538 538 + 539 539 + xrt_pose wrist_xr = wrist; 540 540 + 541 541 + switch (skeleton_hand) { 542 542 + case XRT_HAND_LEFT: { 543 543 + math_quat_rotate(&wrist_xr.orientation, &left_wrist_rotate, &wrist_xr.orientation); 544 544 + break; 545 545 + } 546 546 + case XRT_HAND_RIGHT: { 547 547 + math_quat_rotate(&wrist_xr.orientation, &right_wrist_rotate, &wrist_xr.orientation); 548 548 + break; 370 549 } 371 371 - for (const auto &name : FACE_BUTTONS) { 372 372 - auto *input = get_input_from_name(name); 373 373 - if (input && input->value.boolean) { 374 374 - thumb = 1.f; 375 375 - break; 376 376 - } 377 550 } 378 378 - auto curl_values = u_hand_tracking_curl_values{pinky, ring, middle, index, thumb}; 379 551 380 380 - struct xrt_space_relation hand_relation = {}; 381 381 - m_relation_history_get(relation_hist, desired_timestamp_ns, &hand_relation); 552 552 + joints[XRT_HAND_JOINT_WRIST].relation.pose = wrist_xr; 553 553 + joints[XRT_HAND_JOINT_WRIST].relation.relation_flags = valid_flags; 382 554 383 383 - u_hand_sim_simulate_for_valve_index_knuckles(&curl_values, get_xrt_hand(), &hand_relation, out); 555 555 + xrt_pose parent_pose; 556 556 + for (int joint = XRT_HAND_JOINT_THUMB_METACARPAL; joint <= XRT_HAND_JOINT_LITTLE_TIP; ++joint) { 557 557 + // Luckily openvr and openxr joint values match 558 558 + xrt_pose pose = bone_to_pose(bones[joint]); 559 559 + joints[joint].relation.relation_flags = valid_flags; 560 560 + 561 561 + if (u_hand_joint_is_metacarpal((xrt_hand_joint)joint)) { 562 562 + // Counter transform metacarpals 563 563 + math_pose_transform(&root, &pose, &pose); 564 564 + math_pose_transform(&wrist, &pose, &pose); 565 565 + } else { 566 566 + math_pose_transform(&parent_pose, &pose, &pose); 567 567 + } 568 568 + 569 569 + parent_pose = pose; 384 570 385 385 - struct xrt_relation_chain chain = {}; 571 571 + // Rotate joint to OpenXR orientation 572 572 + switch (skeleton_hand) { 573 573 + case XRT_HAND_LEFT: math_quat_rotate(&pose.orientation, &left_hand_rotate, &pose.orientation); break; 574 574 + case XRT_HAND_RIGHT: math_quat_rotate(&pose.orientation, &right_hand_rotate, &pose.orientation); break; 575 575 + } 576 576 + joints[joint].relation.pose = pose; 577 577 + } 386 578 387 387 - struct xrt_pose pose_offset = XRT_POSE_IDENTITY; 388 388 - vive_poses_get_pose_offset(name, device_type, inputs_map["HAND"]->name, &pose_offset); 579 579 + joints[XRT_HAND_JOINT_PALM].relation.relation_flags = valid_flags; 580 580 + joints[XRT_HAND_JOINT_PALM].relation.pose = 581 581 + generate_palm_pose(joints[XRT_HAND_JOINT_MIDDLE_METACARPAL].relation.pose, 582 582 + joints[XRT_HAND_JOINT_MIDDLE_PROXIMAL].relation.pose); 389 583 390 390 - m_relation_chain_push_pose(&chain, &pose_offset); 391 391 - m_relation_chain_push_relation(&chain, &hand_relation); 392 392 - m_relation_chain_resolve(&chain, &out->hand_pose); 584 584 + u_hand_joints_apply_joint_width(&joint_set); 585 585 + this->joint_history.push_back(JointsWithTimestamp{joint_set, timestamp}); 393 586 } 394 587 395 588 xrt_input * 396 589 Device::get_input_from_name(const std::string_view name) 397 590 { 591 591 + static const std::array ignore_inputs = {"/input/finger/index"sv, "/input/finger/middle"sv, 592 592 + "/input/finger/ring"sv, "/input/finger/pinky"sv, 593 593 + "/input/grip/touch"sv}; 594 594 + 398 595 // Return nullptr without any other output to suppress a pile of useless warnings found below. 399 399 - if (name == "/input/finger/index" || name == "/input/finger/middle" || name == "/input/finger/ring" || 400 400 - name == "/input/finger/pinky") { 596 596 + if (std::ranges::find(ignore_inputs, name) != std::ranges::end(ignore_inputs)) { 401 597 return nullptr; 402 598 } 403 599 auto input = inputs_map.find(name); ··· 447 643 return XRT_SUCCESS; 448 644 } 449 645 450 450 - IndexFingerInput * 451 451 - ControllerDevice::get_finger_from_name(const std::string_view name) 452 452 - { 453 453 - auto finger = finger_inputs_map.find(name); 454 454 - if (finger == finger_inputs_map.end()) { 455 455 - DEV_WARN("requested unknown finger name %s for device %s", std::string(name).c_str(), serial); 456 456 - return nullptr; 457 457 - } 458 458 - return finger->second; 459 459 - } 460 460 - 461 646 xrt_result_t 462 647 ControllerDevice::get_hand_tracking(enum xrt_input_name name, 463 648 int64_t desired_timestamp_ns, 464 649 struct xrt_hand_joint_set *out_value, 465 650 int64_t *out_timestamp_ns) 466 651 { 467 467 - if (!has_index_hand_tracking) 652 652 + if (!has_hand_tracking) { 468 653 return XRT_ERROR_NOT_IMPLEMENTED; 469 469 - update_hand_tracking(desired_timestamp_ns, out_value); 470 470 - out_value->is_active = true; 471 471 - hand_tracking_timestamp = desired_timestamp_ns; 472 472 - *out_timestamp_ns = hand_tracking_timestamp; 654 654 + } 655 655 + 656 656 + // No handtracking data? 657 657 + if (joint_history.empty()) { 658 658 + out_value->is_active = false; 659 659 + return XRT_SUCCESS; 660 660 + } 661 661 + 662 662 + const auto it = std::ranges::lower_bound(joint_history, desired_timestamp_ns, {}, 663 663 + [](const JointsWithTimestamp &joint) { return joint.timestamp; }); 664 664 + 665 665 + auto predict_joint_set = [out_value, out_timestamp_ns, 666 666 + desired_timestamp_ns](const JointsWithTimestamp &joints) { 667 667 + out_value->is_active = joints.joint_set.is_active; 668 668 + int64_t delta_ns = desired_timestamp_ns - joints.timestamp; 669 669 + double delta_s = time_ns_to_s(delta_ns); 670 670 + 671 671 + *out_timestamp_ns = desired_timestamp_ns; 672 672 + m_predict_relation(&joints.joint_set.hand_pose, delta_s, &out_value->hand_pose); 673 673 + for (int i = 0; i < XRT_HAND_JOINT_COUNT; i++) { 674 674 + auto &new_joint = joints.joint_set.values.hand_joint_set_default[i]; 675 675 + auto &interp_joint = out_value->values.hand_joint_set_default[i]; 676 676 + 677 677 + m_predict_relation(&new_joint.relation, delta_s, &interp_joint.relation); 678 678 + interp_joint.radius = new_joint.radius; 679 679 + } 680 680 + }; 681 681 + 682 682 + if (it == joint_history.end()) { 683 683 + // Timestamp is newer than anything in history 684 684 + predict_joint_set(joint_history.back()); 685 685 + } else if (desired_timestamp_ns == it->timestamp) { 686 686 + *out_value = it->joint_set; 687 687 + *out_timestamp_ns = it->timestamp; 688 688 + } else if (it == joint_history.begin()) { 689 689 + // Timestamp is older than anything in history 690 690 + predict_joint_set(joint_history.front()); 691 691 + } else { 692 692 + // Interpolate 693 693 + auto it_previous = it - 1; 694 694 + 695 695 + auto delta_before = desired_timestamp_ns - it_previous->timestamp; 696 696 + auto delta_after = it->timestamp - desired_timestamp_ns; 697 697 + float lerp_amt = (float)delta_before / (float)(delta_after - delta_before); 698 698 + 699 699 + auto interpolate = [lerp_amt](xrt_space_relation &previous, xrt_space_relation &next, 700 700 + xrt_space_relation &out) { 701 701 + auto flags = (xrt_space_relation_flags)(previous.relation_flags & next.relation_flags); 702 702 + m_space_relation_interpolate(&previous, &next, lerp_amt, flags, &out); 703 703 + }; 704 704 + 705 705 + interpolate(it_previous->joint_set.hand_pose, it->joint_set.hand_pose, out_value->hand_pose); 706 706 + 707 707 + for (int i = 0; i < XRT_HAND_JOINT_COUNT; i++) { 708 708 + auto &prev = it_previous->joint_set.values.hand_joint_set_default[i]; 709 709 + auto &next = it->joint_set.values.hand_joint_set_default[i]; 710 710 + auto &out = out_value->values.hand_joint_set_default[i]; 711 711 + interpolate(prev.relation, next.relation, out.relation); 712 712 + out.radius = next.radius; 713 713 + } 714 714 + 715 715 + *out_timestamp_ns = desired_timestamp_ns; 716 716 + } 717 717 + 473 718 return XRT_SUCCESS; 474 719 } 475 720 ··· 527 772 Device::get_pose(at_timestamp_ns, &rel); 528 773 529 774 xrt_pose pose_offset = XRT_POSE_IDENTITY; 530 530 - vive_poses_get_pose_offset(input_class->name, device_type, name, &pose_offset); 531 775 776 776 + if (name == XRT_INPUT_GENERIC_PALM_POSE) { 777 777 + if (!palm_offsets[skeleton_hand].has_value()) { 778 778 + DEV_ERR("%s hand skeleton has not been initialized", 779 779 + skeleton_hand == XRT_HAND_LEFT ? "left" : "right"); 780 780 + *out_relation = XRT_SPACE_RELATION_ZERO; 781 781 + return XRT_SUCCESS; 782 782 + } 783 783 + pose_offset = *palm_offsets[skeleton_hand]; 784 784 + } else { 785 785 + vive_poses_get_pose_offset(input_class->name, device_type, name, &pose_offset); 786 786 + } 532 787 xrt_relation_chain relchain = {}; 533 788 534 789 m_relation_chain_push_pose(&relchain, &pose_offset); ··· 1138 1393 const std::string_view name = {static_cast<char *>(prop.pvBuffer), prop.unBufferSize}; 1139 1394 if (name == "SlimeVR Virtual Tracker\0"sv) { 1140 1395 static const InputClass input_class = { 1141 1141 - XRT_DEVICE_VIVE_TRACKER, {XRT_INPUT_GENERIC_TRACKER_POSE}, {}, {}}; 1396 1396 + XRT_DEVICE_VIVE_TRACKER, {XRT_INPUT_GENERIC_TRACKER_POSE}, {}}; 1142 1397 this->name = input_class.name; 1143 1398 set_input_class(&input_class); 1144 1399 this->manufacturer = name.substr(0, name.find_first_of(' ')); ··· 1157 1412 } 1158 1413 case vr::TrackedControllerRole_RightHand: { 1159 1414 this->device_type = XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER; 1160 1160 - set_hand_tracking_hand(XRT_INPUT_HT_CONFORMING_RIGHT); 1415 1415 + set_active_hand(XRT_HAND_RIGHT); 1161 1416 break; 1162 1417 } 1163 1418 case vr::TrackedControllerRole_LeftHand: { 1164 1419 this->device_type = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER; 1165 1165 - set_hand_tracking_hand(XRT_INPUT_HT_CONFORMING_LEFT); 1420 1420 + set_active_hand(XRT_HAND_LEFT); 1166 1421 break; 1167 1422 } 1168 1423 case vr::TrackedControllerRole_OptOut: {

+24 -10

src/xrt/drivers/steamvr_lh/device.hpp

··· 11 11 12 12 #include "math/m_relation_history.h" 13 13 14 14 + #include "util/u_template_historybuf.hpp" 14 15 #include "xrt/xrt_device.h" 15 16 16 17 #include "vive/vive_common.h" ··· 23 24 #include <vector> 24 25 #include <memory> 25 26 #include <unordered_map> 27 27 + #include <span> 28 28 + #include <array> 29 29 + #include <optional> 26 30 27 31 #include <condition_variable> 28 32 #include <mutex> ··· 234 238 xrt_result_t 235 239 get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) override; 236 240 237 237 - IndexFingerInput * 238 238 - get_finger_from_name(std::string_view name); 239 239 - 240 241 xrt_result_t 241 242 get_hand_tracking(enum xrt_input_name name, 242 243 int64_t desired_timestamp_ns, ··· 247 248 get_xrt_hand(); 248 249 249 250 void 250 250 - update_hand_tracking(int64_t desired_timestamp_ns, struct xrt_hand_joint_set *out); 251 251 + update_skeleton_transforms(std::span<const vr::VRBoneTransform_t> bones); 252 252 + 253 253 + void 254 254 + set_skeleton(std::span<const vr::VRBoneTransform_t> bones, xrt_hand hand, bool is_simulated, const char *path); 255 255 + 256 256 + void 257 257 + set_active_hand(xrt_hand hand); 251 258 252 259 protected: 253 260 void 254 261 set_input_class(const InputClass *input_class); 255 262 263 263 + void 264 264 + generate_palm_pose_offset(std::span<const vr::VRBoneTransform_t> bones, xrt_hand hand); 265 265 + 256 266 private: 257 267 vr::VRInputComponentHandle_t haptic_handle{0}; 258 268 std::unique_ptr<xrt_output> output{nullptr}; 259 259 - bool has_index_hand_tracking{false}; 260 260 - std::vector<IndexFingerInput> finger_inputs_vec; 261 261 - std::unordered_map<std::string_view, IndexFingerInput *> finger_inputs_map; 262 262 - uint64_t hand_tracking_timestamp; 269 269 + bool has_hand_tracking{false}; 270 270 + xrt_hand skeleton_hand = XRT_HAND_LEFT; 271 271 + std::array<std::optional<xrt_pose>, 2> palm_offsets; 272 272 + std::array<xrt_input, 2> hand_tracking_inputs{}; 263 273 264 264 - void 265 265 - set_hand_tracking_hand(xrt_input_name name); 274 274 + struct JointsWithTimestamp 275 275 + { 276 276 + xrt_hand_joint_set joint_set; 277 277 + int64_t timestamp{0}; 278 278 + }; 279 279 + xrt::auxiliary::util::HistoryBuffer<JointsWithTimestamp, 5> joint_history; 266 280 267 281 vr::ETrackedPropertyError 268 282 handle_property_write(const vr::PropertyWrite_t &prop) override;

+2 -17

src/xrt/drivers/steamvr_lh/interfaces/context.hpp

··· 28 28 29 29 #include "xrt/xrt_tracking.h" 30 30 31 31 - enum IndexFinger 32 32 - { 33 33 - Invalid = -1, 34 34 - Index = 1, 35 35 - Middle, 36 36 - Ring, 37 37 - Pinky, 38 38 - }; 39 39 - 40 40 - struct IndexFingerInput 41 41 - { 42 42 - int64_t timestamp; 43 43 - IndexFinger finger; 44 44 - float value; 45 45 - }; 46 46 - 47 31 struct xrt_input; 48 32 class Device; 33 33 + class ControllerDevice; 49 34 class Context final : public xrt_tracking_origin, 50 35 public vr::IVRDriverContext, 51 36 public vr::IVRServerDriverHost, ··· 70 55 71 56 std::vector<vr::VRInputComponentHandle_t> handles; 72 57 std::unordered_map<vr::VRInputComponentHandle_t, xrt_input *> handle_to_input; 73 73 - std::unordered_map<vr::VRInputComponentHandle_t, IndexFingerInput *> handle_to_finger; 74 58 struct Vec2Components 75 59 { 76 60 vr::VRInputComponentHandle_t x; ··· 78 62 }; 79 63 std::unordered_map<vr::VRInputComponentHandle_t, Vec2Components *> vec2_inputs; 80 64 std::unordered_map<xrt_input *, std::unique_ptr<Vec2Components>> vec2_input_to_components; 65 65 + std::unordered_map<vr::VRInputComponentHandle_t, ControllerDevice *> skeleton_to_controller; 81 66 82 67 struct Event 83 68 {

+54 -24

src/xrt/drivers/steamvr_lh/steamvr_lh.cpp

··· 482 482 return vr::VRInputError_InvalidHandle; 483 483 } 484 484 if (xrt_input *input = device->get_input_from_name(name); input) { 485 485 - CTX_DEBUG("creating component %s", name); 485 485 + CTX_DEBUG("creating component %s for %p", name, (void *)device); 486 486 vr::VRInputComponentHandle_t handle = new_handle(); 487 487 handle_to_input[handle] = input; 488 488 *pHandle = handle; 489 489 - } else if (device != hmd) { 490 490 - auto *controller = static_cast<ControllerDevice *>(device); 491 491 - if (IndexFingerInput *finger = controller->get_finger_from_name(name); finger) { 492 492 - CTX_DEBUG("creating finger component %s", name); 493 493 - vr::VRInputComponentHandle_t handle = new_handle(); 494 494 - handle_to_finger[handle] = finger; 495 495 - *pHandle = handle; 496 496 - } 497 489 } 498 490 return vr::VRInputError_None; 499 491 } ··· 596 588 } else { 597 589 input->value.vec1.x = fNewValue; 598 590 } 599 599 - } else { 600 600 - if (ulComponent != vr::k_ulInvalidInputComponentHandle) { 601 601 - if (auto finger_input = handle_to_finger.find(ulComponent); 602 602 - finger_input != handle_to_finger.end() && finger_input->second) { 603 603 - auto now = std::chrono::steady_clock::now(); 604 604 - std::chrono::duration<double, std::chrono::seconds::period> offset_dur(fTimeOffset); 605 605 - std::chrono::duration offset = (now + offset_dur).time_since_epoch(); 606 606 - int64_t timestamp = 607 607 - std::chrono::duration_cast<std::chrono::nanoseconds>(offset).count(); 608 608 - finger_input->second->timestamp = timestamp; 609 609 - finger_input->second->value = fNewValue; 610 610 - } else { 611 611 - CTX_WARN("Unmapped component %" PRIu64, ulComponent); 612 612 - } 613 613 - } 614 591 } 615 592 return vr::VRInputError_None; 616 593 } ··· 652 629 uint32_t unGripLimitTransformCount, 653 630 vr::VRInputComponentHandle_t *pHandle) 654 631 { 632 632 + std::string_view path(pchSkeletonPath); // should be /skeleton/hand/left or /skeleton/hand/right 633 633 + std::string_view skeleton_pfx("/skeleton/hand/"); 634 634 + if (!path.starts_with(skeleton_pfx)) { 635 635 + CTX_ERR("Got invalid skeleton path: %s", std::string(path).c_str()); 636 636 + return vr::VRInputError_InvalidSkeleton; 637 637 + } 638 638 + 639 639 + if (auto ret = create_component_common(ulContainer, pchSkeletonPath, pHandle); ret != vr::VRInputError_None) { 640 640 + return ret; 641 641 + } 642 642 + 643 643 + auto *device = static_cast<ControllerDevice *>(prop_container_to_device(ulContainer)); 644 644 + path.remove_prefix(skeleton_pfx.size()); 645 645 + xrt_hand hand; 646 646 + if (path == "left") { 647 647 + hand = XRT_HAND_LEFT; 648 648 + } else if (path == "right") { 649 649 + hand = XRT_HAND_RIGHT; 650 650 + } else { 651 651 + CTX_ERR("Got invalid skeleton path suffix: %s", std::string(path).c_str()); 652 652 + return vr::VRInputError_InvalidSkeleton; 653 653 + } 654 654 + 655 655 + device->set_skeleton(std::span(pGripLimitTransforms, unGripLimitTransformCount), hand, 656 656 + eSkeletalTrackingLevel == vr::VRSkeletalTracking_Estimated, pchSkeletonPath); 657 657 + skeleton_to_controller[*pHandle] = device; 658 658 + 655 659 return vr::VRInputError_None; 656 660 } 657 661 ··· 661 665 const vr::VRBoneTransform_t *pTransforms, 662 666 uint32_t unTransformCount) 663 667 { 668 668 + if (eMotionRange != vr::VRSkeletalMotionRange_WithoutController) { 669 669 + return vr::VRInputError_None; 670 670 + } 671 671 + 672 672 + if (!update_component_common(ulComponent, 0)) { 673 673 + return vr::VRInputError_InvalidHandle; 674 674 + } 675 675 + 676 676 + auto *device = skeleton_to_controller[ulComponent]; 677 677 + if (!device) { 678 678 + CTX_ERR("Got unknown component handle %lu", ulComponent); 679 679 + return vr::VRInputError_InvalidHandle; 680 680 + } 681 681 + 682 682 + device->update_skeleton_transforms(std::span(pTransforms, unTransformCount)); 683 683 + 664 684 return vr::VRInputError_None; 665 685 } 666 686 ··· 757 777 out_roles->left = left; 758 778 out_roles->right = right; 759 779 out_roles->gamepad = gamepad; 780 780 + 781 781 + if (left != XRT_DEVICE_ROLE_UNASSIGNED) { 782 782 + auto *left_dev = static_cast<ControllerDevice *>(xsysd->xdevs[left]); 783 783 + left_dev->set_active_hand(XRT_HAND_LEFT); 784 784 + } 785 785 + 786 786 + if (right != XRT_DEVICE_ROLE_UNASSIGNED) { 787 787 + auto *right_dev = static_cast<ControllerDevice *>(xsysd->xdevs[right]); 788 788 + right_dev->set_active_hand(XRT_HAND_RIGHT); 789 789 + } 760 790 } 761 791 762 792 return XRT_SUCCESS;