/* * WiVRn VR streaming * Copyright (C) 2022 Guillaume Meunier * Copyright (C) 2022 Patrick Nicolas * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ #include "wivrn_controller.h" #include "hand_kinematics.h" #include "wivrn_session.h" #include "util/u_logging.h" #include #include #include "xrt/xrt_defines.h" namespace wivrn { enum wivrn_controller_input_index { WIVRN_CONTROLLER_AIM_POSE, WIVRN_CONTROLLER_GRIP_POSE, WIVRN_CONTROLLER_HAND_TRACKER, WIVRN_CONTROLLER_PALM_POSE, WIVRN_CONTROLLER_MENU_CLICK, // /user/hand/left/input/menu/click WIVRN_CONTROLLER_A_CLICK, // /user/hand/right/input/a/click WIVRN_CONTROLLER_A_TOUCH, // /user/hand/right/input/a/touch WIVRN_CONTROLLER_B_CLICK, // /user/hand/right/input/b/click WIVRN_CONTROLLER_B_TOUCH, // /user/hand/right/input/b/touch WIVRN_CONTROLLER_X_CLICK = WIVRN_CONTROLLER_A_CLICK, // /user/hand/left/input/x/click WIVRN_CONTROLLER_X_TOUCH = WIVRN_CONTROLLER_A_TOUCH, // /user/hand/left/input/x/touch WIVRN_CONTROLLER_Y_CLICK = WIVRN_CONTROLLER_B_CLICK, // /user/hand/left/input/y/click WIVRN_CONTROLLER_Y_TOUCH = WIVRN_CONTROLLER_B_TOUCH, // /user/hand/left/input/y/touch WIVRN_CONTROLLER_SQUEEZE_CLICK, // /user/hand/XXXX/input/squeeze/click WIVRN_CONTROLLER_SQUEEZE_VALUE, // /user/hand/XXXX/input/squeeze/value WIVRN_CONTROLLER_TRIGGER_CLICK, // /user/hand/XXXX/input/trigger/click WIVRN_CONTROLLER_TRIGGER_VALUE, // /user/hand/XXXX/input/trigger/value WIVRN_CONTROLLER_TRIGGER_TOUCH, // /user/hand/XXXX/input/trigger/touch WIVRN_CONTROLLER_THUMBSTICK, // /user/hand/XXXX/input/thumbstick/{x,y} WIVRN_CONTROLLER_THUMBSTICK_CLICK, // /user/hand/XXXX/input/thumbstick/click WIVRN_CONTROLLER_THUMBSTICK_TOUCH, // /user/hand/XXXX/input/thumbstick/touch WIVRN_CONTROLLER_THUMBREST_TOUCH, // /user/hand/XXXX/input/thumbrest/touch WIVRN_CONTROLLER_INPUT_COUNT }; enum class wivrn_input_type { BOOL, FLOAT, VEC2_X, VEC2_Y }; struct wivrn_to_wivrn_controller_input { wivrn_controller_input_index input_id; device_id wivrn_id; wivrn_input_type input_type; }; // clang-format off static const wivrn_to_wivrn_controller_input left_hand_bindings[] = { {WIVRN_CONTROLLER_MENU_CLICK, device_id::MENU_CLICK, wivrn_input_type::BOOL}, // /user/hand/left/input/menu/click {WIVRN_CONTROLLER_X_CLICK, device_id::X_CLICK, wivrn_input_type::BOOL}, // /user/hand/left/input/x/click {WIVRN_CONTROLLER_X_TOUCH, device_id::X_TOUCH, wivrn_input_type::BOOL}, // /user/hand/left/input/x/touch {WIVRN_CONTROLLER_Y_CLICK, device_id::Y_CLICK, wivrn_input_type::BOOL}, // /user/hand/left/input/y/click {WIVRN_CONTROLLER_Y_TOUCH, device_id::Y_TOUCH, wivrn_input_type::BOOL}, // /user/hand/left/input/y/touch {WIVRN_CONTROLLER_MENU_CLICK, device_id::MENU_CLICK, wivrn_input_type::BOOL}, // /user/hand/left/input/menu/click {WIVRN_CONTROLLER_SQUEEZE_VALUE, device_id::LEFT_SQUEEZE_VALUE, wivrn_input_type::FLOAT}, // /user/hand/left/input/squeeze/value {WIVRN_CONTROLLER_TRIGGER_VALUE, device_id::LEFT_TRIGGER_VALUE, wivrn_input_type::FLOAT}, // /user/hand/left/input/trigger/value {WIVRN_CONTROLLER_TRIGGER_TOUCH, device_id::LEFT_TRIGGER_TOUCH, wivrn_input_type::BOOL}, // /user/hand/left/input/trigger/touch {WIVRN_CONTROLLER_THUMBSTICK, device_id::LEFT_THUMBSTICK_X, wivrn_input_type::VEC2_X}, // /user/hand/left/input/thumbstick/x {WIVRN_CONTROLLER_THUMBSTICK, device_id::LEFT_THUMBSTICK_Y, wivrn_input_type::VEC2_Y}, // /user/hand/left/input/thumbstick/y {WIVRN_CONTROLLER_THUMBSTICK_CLICK, device_id::LEFT_THUMBSTICK_CLICK, wivrn_input_type::BOOL},// /user/hand/left/input/thumbstick/click {WIVRN_CONTROLLER_THUMBSTICK_TOUCH, device_id::LEFT_THUMBSTICK_TOUCH, wivrn_input_type::BOOL},// /user/hand/left/input/thumbstick/touch {WIVRN_CONTROLLER_THUMBREST_TOUCH, device_id::LEFT_THUMBREST_TOUCH, wivrn_input_type::BOOL}, // /user/hand/left/input/thumbrest/touch }; static const wivrn_to_wivrn_controller_input right_hand_bindings[] = { {WIVRN_CONTROLLER_A_CLICK, device_id::A_CLICK, wivrn_input_type::BOOL}, // /user/hand/right/input/a/click {WIVRN_CONTROLLER_A_TOUCH, device_id::A_TOUCH, wivrn_input_type::BOOL}, // /user/hand/right/input/a/touch {WIVRN_CONTROLLER_B_CLICK, device_id::B_CLICK, wivrn_input_type::BOOL}, // /user/hand/right/input/b/click {WIVRN_CONTROLLER_B_TOUCH, device_id::B_TOUCH, wivrn_input_type::BOOL}, // /user/hand/right/input/b/touch {WIVRN_CONTROLLER_SQUEEZE_VALUE, device_id::RIGHT_SQUEEZE_VALUE, wivrn_input_type::FLOAT}, // /user/hand/right/input/squeeze/value {WIVRN_CONTROLLER_TRIGGER_VALUE, device_id::RIGHT_TRIGGER_VALUE, wivrn_input_type::FLOAT}, // /user/hand/right/input/trigger/value {WIVRN_CONTROLLER_TRIGGER_TOUCH, device_id::RIGHT_TRIGGER_TOUCH, wivrn_input_type::BOOL}, // /user/hand/right/input/trigger/touch {WIVRN_CONTROLLER_THUMBSTICK, device_id::RIGHT_THUMBSTICK_X, wivrn_input_type::VEC2_X}, // /user/hand/right/input/thumbstick/x {WIVRN_CONTROLLER_THUMBSTICK, device_id::RIGHT_THUMBSTICK_Y, wivrn_input_type::VEC2_Y}, // /user/hand/right/input/thumbstick/y {WIVRN_CONTROLLER_THUMBSTICK_CLICK, device_id::RIGHT_THUMBSTICK_CLICK, wivrn_input_type::BOOL},// /user/hand/right/input/thumbstick/click {WIVRN_CONTROLLER_THUMBSTICK_TOUCH, device_id::RIGHT_THUMBSTICK_TOUCH, wivrn_input_type::BOOL},// /user/hand/right/input/thumbstick/touch {WIVRN_CONTROLLER_THUMBREST_TOUCH, device_id::RIGHT_THUMBREST_TOUCH, wivrn_input_type::BOOL}, // /user/hand/right/input/thumbrest/touch }; // clang-format on static const size_t left_hand_bindings_count = ARRAY_SIZE(left_hand_bindings); static const size_t right_hand_bindings_count = ARRAY_SIZE(right_hand_bindings); static xrt_binding_input_pair simple_input_binding[] = { {XRT_INPUT_SIMPLE_SELECT_CLICK, XRT_INPUT_TOUCH_TRIGGER_VALUE}, {XRT_INPUT_SIMPLE_MENU_CLICK, XRT_INPUT_TOUCH_MENU_CLICK}, {XRT_INPUT_SIMPLE_GRIP_POSE, XRT_INPUT_TOUCH_GRIP_POSE}, {XRT_INPUT_SIMPLE_AIM_POSE, XRT_INPUT_TOUCH_AIM_POSE}, }; static xrt_binding_output_pair simple_output_binding[] = { {XRT_OUTPUT_NAME_SIMPLE_VIBRATION, XRT_OUTPUT_NAME_TOUCH_HAPTIC}, }; static xrt_binding_profile wivrn_binding_profiles[] = { { .name = XRT_DEVICE_SIMPLE_CONTROLLER, .inputs = simple_input_binding, .input_count = std::size(simple_input_binding), .outputs = simple_output_binding, .output_count = std::size(simple_output_binding), }, }; static void wivrn_controller_destroy(xrt_device * xdev); static xrt_result_t wivrn_controller_get_tracked_pose(xrt_device * xdev, xrt_input_name name, int64_t at_timestamp_ns, xrt_space_relation * out_relation) { *out_relation = static_cast(xdev)->get_tracked_pose(name, at_timestamp_ns); return XRT_SUCCESS; } static void wivrn_controller_get_hand_tracking(xrt_device * xdev, xrt_input_name name, int64_t desired_timestamp_ns, xrt_hand_joint_set * out_value, int64_t * out_timestamp_ns); static void wivrn_controller_set_output(struct xrt_device * xdev, enum xrt_output_name name, const union xrt_output_value * value); static xrt_result_t wivrn_controller_update_inputs(xrt_device * xdev) { static_cast(xdev)->update_inputs(); return XRT_SUCCESS; } wivrn_controller::wivrn_controller(int hand_id, xrt_device * hmd, wivrn::wivrn_session * cnx) : xrt_device{}, grip(hand_id == 0 ? device_id::LEFT_GRIP : device_id::RIGHT_GRIP), aim(hand_id == 0 ? device_id::LEFT_AIM : device_id::RIGHT_AIM), palm(hand_id == 0 ? device_id::LEFT_PALM : device_id::RIGHT_PALM), joints(hand_id), cnx(cnx) { xrt_device * base = this; base->destroy = wivrn_controller_destroy; base->get_tracked_pose = wivrn_controller_get_tracked_pose; base->get_hand_tracking = wivrn_controller_get_hand_tracking; base->set_output = wivrn_controller_set_output; base->update_inputs = wivrn_controller_update_inputs; base->name = XRT_DEVICE_TOUCH_CONTROLLER; base->orientation_tracking_supported = true; base->hand_tracking_supported = cnx->get_info().hand_tracking; base->position_tracking_supported = true; base->tracking_origin = hmd->tracking_origin; inputs_array.resize(WIVRN_CONTROLLER_INPUT_COUNT); inputs = inputs_array.data(); input_count = inputs_array.size(); // Setup input. #define SET_INPUT(NAME) \ do \ { \ inputs[WIVRN_CONTROLLER_##NAME].name = XRT_INPUT_TOUCH_##NAME; \ inputs[WIVRN_CONTROLLER_##NAME].active = true; \ } while (0) SET_INPUT(AIM_POSE); SET_INPUT(GRIP_POSE); if (hand_id == 0) { SET_INPUT(X_CLICK); SET_INPUT(Y_CLICK); SET_INPUT(X_TOUCH); SET_INPUT(Y_TOUCH); SET_INPUT(MENU_CLICK); } else { SET_INPUT(A_CLICK); SET_INPUT(B_CLICK); SET_INPUT(A_TOUCH); SET_INPUT(B_TOUCH); } SET_INPUT(SQUEEZE_VALUE); SET_INPUT(TRIGGER_VALUE); SET_INPUT(TRIGGER_TOUCH); SET_INPUT(THUMBSTICK); SET_INPUT(THUMBSTICK_CLICK); SET_INPUT(THUMBSTICK_TOUCH); SET_INPUT(THUMBREST_TOUCH); inputs[WIVRN_CONTROLLER_HAND_TRACKER].name = hand_id == 0 ? XRT_INPUT_GENERIC_HAND_TRACKING_LEFT : XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT; inputs[WIVRN_CONTROLLER_HAND_TRACKER].active = hand_tracking_supported; inputs[WIVRN_CONTROLLER_PALM_POSE].name = XRT_INPUT_GENERIC_PALM_POSE; inputs[WIVRN_CONTROLLER_PALM_POSE].active = cnx->get_info().palm_pose; output_count = 1; outputs = &haptic_output; haptic_output.name = XRT_OUTPUT_NAME_TOUCH_HAPTIC; inputs_staging = inputs_array; switch (hand_id) { case 0: device_type = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER; // Print name. strcpy(str, "WiVRn HMD left hand controller"); strcpy(serial, "WiVRn HMD left hand controller"); break; case 1: device_type = XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER; // Print name. strcpy(str, "WiVRn HMD right hand controller"); strcpy(serial, "WiVRn HMD right hand controller"); break; default: throw std::runtime_error("Invalid hand ID"); } binding_profile_count = std::size(wivrn_binding_profiles); binding_profiles = wivrn_binding_profiles; } void wivrn_controller::update_inputs() { std::lock_guard _{mutex}; inputs_array = inputs_staging; } void wivrn_controller::set_inputs(const from_headset::inputs & inputs, const clock_offset & clock_offset) { std::lock_guard lock{mutex}; for (const auto & input: inputs.values) { set_inputs(input.id, input.value, input.last_change_time ? clock_offset.from_headset(input.last_change_time) : 0); } } void wivrn_controller::set_inputs(device_id input_id, float value, int64_t last_change_time) { const struct wivrn_to_wivrn_controller_input * bindings; size_t bindings_count; if (device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER) { bindings = left_hand_bindings; bindings_count = left_hand_bindings_count; } else { bindings = right_hand_bindings; bindings_count = right_hand_bindings_count; } const struct wivrn_to_wivrn_controller_input * binding = NULL; for (size_t i = 0; i < bindings_count; i++) { if (bindings[i].wivrn_id == input_id) { binding = &bindings[i]; break; } } if (binding) { inputs_staging[binding->input_id].timestamp = last_change_time; switch (binding->input_type) { case wivrn_input_type::BOOL: inputs_staging[binding->input_id].value.boolean = (value != 0); break; case wivrn_input_type::FLOAT: inputs_staging[binding->input_id].value.vec1.x = value; break; case wivrn_input_type::VEC2_X: inputs_staging[binding->input_id].value.vec2.x = value; break; case wivrn_input_type::VEC2_Y: inputs_staging[binding->input_id].value.vec2.y = value; break; } } } xrt_space_relation wivrn_controller::get_tracked_pose(xrt_input_name name, int64_t at_timestamp_ns) { std::chrono::nanoseconds extrapolation_time; xrt_space_relation res; switch (name) { case XRT_INPUT_TOUCH_AIM_POSE: std::tie(extrapolation_time, res) = aim.get_at(at_timestamp_ns); cnx->set_enabled(aim.device, true); break; case XRT_INPUT_TOUCH_GRIP_POSE: std::tie(extrapolation_time, res) = grip.get_at(at_timestamp_ns); cnx->set_enabled(grip.device, true); break; case XRT_INPUT_GENERIC_PALM_POSE: std::tie(extrapolation_time, res) = palm.get_at(at_timestamp_ns); cnx->set_enabled(palm.device, true); break; default: U_LOG_W("Unknown input name requested"); return {}; } cnx->add_predict_offset(extrapolation_time); return res; } std::pair wivrn_controller::get_hand_tracking(xrt_input_name name, int64_t desired_timestamp_ns) { switch (name) { case XRT_INPUT_GENERIC_HAND_TRACKING_LEFT: case XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT: { auto [extrapolation_time, data] = joints.get_at(desired_timestamp_ns); cnx->add_predict_offset(extrapolation_time); cnx->set_enabled(joints.hand_id == 0 ? to_headset::tracking_control::id::left_hand : to_headset::tracking_control::id::right_hand, true); return {data, desired_timestamp_ns}; } default: U_LOG_W("Unknown input name requested"); return {}; } } void wivrn_controller::update_tracking(const from_headset::tracking & tracking, const clock_offset & offset) { if (not aim.update_tracking(tracking, offset)) cnx->set_enabled(aim.device, false); if (not grip.update_tracking(tracking, offset)) cnx->set_enabled(grip.device, false); if (not palm.update_tracking(tracking, offset)) cnx->set_enabled(palm.device, false); } void wivrn_controller::update_hand_tracking(const from_headset::hand_tracking & tracking, const clock_offset & offset) { // if (not joints.update_tracking(tracking, offset)) // cnx->set_enabled(joints.hand_id == 0 ? to_headset::tracking_control::id::left_hand : to_headset::tracking_control::id::right_hand, false); } void wivrn_controller::update_packed_hand_tracking(const from_headset::packed_hand_tracking & packed_hand_tracking, const clock_offset & offset) { auto id = device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER ? from_headset::packed_hand_tracking::left : from_headset::packed_hand_tracking::right; if (id != packed_hand_tracking.hand) return; from_headset::hand_tracking tracking{ .production_timestamp = packed_hand_tracking.production_timestamp, .timestamp = packed_hand_tracking.timestamp, .hand = (from_headset::hand_tracking::hand_id)packed_hand_tracking.hand, }; hand_kinematics hand; auto unpacked = hand.unpack(hand_constants.constants, packed_hand_tracking.pose); tracking.joints.emplace(); uint8_t flags = from_headset::hand_tracking::flags::orientation_valid | from_headset::hand_tracking::flags::orientation_tracked | from_headset::hand_tracking::flags::position_valid | from_headset::hand_tracking::flags::position_tracked; if (packed_hand_tracking.velocity_valid) flags |= from_headset::hand_tracking::flags::angular_velocity_valid | from_headset::hand_tracking::flags::linear_velocity_valid; for (auto [i, j, k]: std::views::zip(*tracking.joints, unpacked, hand_constants.radius)) { i.flags = flags; i.radius = k; i.pose.position.x = j.position[0]; i.pose.position.y = j.position[1]; i.pose.position.z = j.position[2]; i.pose.orientation.x = j.rotation[0]; i.pose.orientation.y = j.rotation[1]; i.pose.orientation.z = j.rotation[2]; i.pose.orientation.w = j.rotation[3]; if (packed_hand_tracking.velocity_valid) { i.linear_velocity.x = j.linear_velocity[0]; i.linear_velocity.y = j.linear_velocity[1]; i.linear_velocity.z = j.linear_velocity[2]; i.angular_velocity.x = j.angular_velocity[0]; i.angular_velocity.y = j.angular_velocity[1]; i.angular_velocity.z = j.angular_velocity[2]; } } if (not joints.update_tracking(tracking, offset)) cnx->set_enabled(joints.hand_id == 0 ? to_headset::tracking_control::id::left_hand : to_headset::tracking_control::id::right_hand, false); } void wivrn_controller::update_packed_hand_tracking(const from_headset::hand_tracking_constants & hand_tracking_constants) { auto id = device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER ? from_headset::hand_tracking_constants::left : from_headset::hand_tracking_constants::right; if (id == hand_tracking_constants.hand) hand_constants = hand_tracking_constants; } void wivrn_controller::set_output(xrt_output_name name, const xrt_output_value * value) { auto id = device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER ? device_id::LEFT_CONTROLLER_HAPTIC : device_id::RIGHT_CONTROLLER_HAPTIC; try { cnx->send_stream(to_headset::haptics{ .id = id, .duration = std::chrono::nanoseconds(value->vibration.duration_ns), .frequency = value->vibration.frequency, .amplitude = value->vibration.amplitude}); } catch (...) { // Ignore errors } } /* * * Functions * */ static void wivrn_controller_destroy(xrt_device * xdev) { static_cast(xdev)->unregister(); } static void wivrn_controller_get_hand_tracking(xrt_device * xdev, xrt_input_name name, int64_t desired_timestamp_ns, xrt_hand_joint_set * out_value, int64_t * out_timestamp_ns) { std::tie(*out_value, *out_timestamp_ns) = static_cast(xdev)->get_hand_tracking(name, desired_timestamp_ns); } static void wivrn_controller_set_output(struct xrt_device * xdev, enum xrt_output_name name, const union xrt_output_value * value) { static_cast(xdev)->set_output(name, value); } } // namespace wivrn