From 76f96686d1565cb490cef44d19860222d8960cde Mon Sep 17 00:00:00 2001 From: danwillm Date: Mon, 1 Aug 2022 18:32:22 +0100 Subject: [PATCH] xrt/drivers: Convert hand tracking drivers to use hand simulation Co-authored-by: Moses Turner --- .../drivers/opengloves/opengloves_device.c | 6 +- src/xrt/drivers/remote/r_device.c | 9 +- src/xrt/drivers/survive/survive_driver.c | 86 ++++++++++++------- src/xrt/drivers/vive/vive_controller.c | 7 +- 4 files changed, 59 insertions(+), 49 deletions(-) diff --git a/src/xrt/drivers/opengloves/opengloves_device.c b/src/xrt/drivers/opengloves/opengloves_device.c index c2833e81d..65940431e 100644 --- a/src/xrt/drivers/opengloves/opengloves_device.c +++ b/src/xrt/drivers/opengloves/opengloves_device.c @@ -28,6 +28,7 @@ DEBUG_GET_ONCE_LOG_OPTION(opengloves_log, "OPENGLOVES_LOG", U_LOGGING_INFO) #include "os/os_threading.h" +#include "util/u_hand_simulation.h" #define OPENGLOVES_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__) #define OPENGLOVES_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__) @@ -120,7 +121,7 @@ opengloves_device_get_hand_tracking(struct xrt_device *xdev, memcpy(values.index.joint_curls, od->last_input->flexion[1], sizeof(od->last_input->flexion[1])); memcpy(values.thumb.joint_curls, od->last_input->flexion[0], sizeof(od->last_input->flexion[0])); - u_hand_joints_update(&od->hand_tracking, hand, requested_timestamp_ns, &values); + u_hand_sim_simulate_generic(&values, hand, &out_joint_set->hand_pose, out_joint_set); struct xrt_space_relation controller_relation = {.pose = {.orientation.w = 1.0f, .position = {0, 0, 0}}}; controller_relation.relation_flags = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | @@ -129,8 +130,6 @@ opengloves_device_get_hand_tracking(struct xrt_device *xdev, struct xrt_pose hand_on_handle_pose; u_hand_joints_offset_valve_index_controller(hand, &static_offset, &hand_on_handle_pose); - u_hand_joints_set_out_data(&od->hand_tracking, hand, &controller_relation, &hand_on_handle_pose, out_joint_set); - *out_timestamp_ns = requested_timestamp_ns; out_joint_set->is_active = true; @@ -247,7 +246,6 @@ opengloves_device_create(struct opengloves_communication_device *ocd, enum xrt_h od->base.inputs[OPENGLOVES_INDEX_HAND_TRACKING].name = od->hand == XRT_HAND_LEFT ? XRT_INPUT_GENERIC_HAND_TRACKING_LEFT : XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT; - u_hand_joints_init_default_set(&od->hand_tracking, hand, XRT_HAND_TRACKING_MODEL_INTRINSIC, 1.0); od->base.hand_tracking_supported = true; // inputs diff --git a/src/xrt/drivers/remote/r_device.c b/src/xrt/drivers/remote/r_device.c index be1871d2d..5b0bb4e1b 100644 --- a/src/xrt/drivers/remote/r_device.c +++ b/src/xrt/drivers/remote/r_device.c @@ -20,6 +20,7 @@ #include "math/m_api.h" #include "r_internal.h" +#include "util/u_hand_simulation.h" #include @@ -156,14 +157,11 @@ r_device_get_hand_tracking(struct xrt_device *xdev, }; enum xrt_hand hand = rd->is_left ? XRT_HAND_LEFT : XRT_HAND_RIGHT; - u_hand_joints_update_curl(&rd->hand_tracking, hand, requested_timestamp_ns, &values); - - struct xrt_pose hand_on_handle_pose = XRT_POSE_IDENTITY; + u_hand_sim_simulate_for_valve_index_knuckles(&values, hand, &out_value->hand_pose, out_value); struct xrt_space_relation relation; xrt_device_get_tracked_pose(xdev, XRT_INPUT_INDEX_GRIP_POSE, requested_timestamp_ns, &relation); - u_hand_joints_set_out_data(&rd->hand_tracking, hand, &relation, &hand_on_handle_pose, out_value); out_value->is_active = latest->hand_tracking_active; // This is a lie @@ -258,9 +256,6 @@ r_device_create(struct r_hub *r, bool is_left) rd->base.device_type = XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER; } - enum xrt_hand hand = rd->is_left ? XRT_HAND_LEFT : XRT_HAND_RIGHT; - u_hand_joints_init_default_set(&rd->hand_tracking, hand, XRT_HAND_TRACKING_MODEL_INTRINSIC, 1.0); - // Setup variable tracker. u_var_add_root(rd, rd->base.str, true); diff --git a/src/xrt/drivers/survive/survive_driver.c b/src/xrt/drivers/survive/survive_driver.c index ae965b797..8eabd1934 100644 --- a/src/xrt/drivers/survive/survive_driver.c +++ b/src/xrt/drivers/survive/survive_driver.c @@ -18,6 +18,7 @@ #include #include "math/m_api.h" +#include "math/m_space.h" #include "tracking/t_tracking.h" #include "xrt/xrt_device.h" #include "util/u_debug.h" @@ -40,6 +41,7 @@ #include "util/u_json.h" #include "util/u_hand_tracking.h" +#include "util/u_hand_simulation.h" #include "util/u_logging.h" #include "math/m_relation_history.h" @@ -388,6 +390,34 @@ survive_controller_device_set_output(struct xrt_device *xdev, } } +struct Button +{ + enum input_index click; + enum input_index touch; +}; + +struct Button buttons[255] = { + [SURVIVE_BUTTON_A] = {.click = VIVE_CONTROLLER_A_CLICK, .touch = VIVE_CONTROLLER_A_TOUCH}, + [SURVIVE_BUTTON_B] = {.click = VIVE_CONTROLLER_B_CLICK, .touch = VIVE_CONTROLLER_B_TOUCH}, + + [SURVIVE_BUTTON_TRACKPAD] = {.click = VIVE_CONTROLLER_TRACKPAD_CLICK, .touch = VIVE_CONTROLLER_TRACKPAD_TOUCH}, + + [SURVIVE_BUTTON_THUMBSTICK] = {.click = VIVE_CONTROLLER_THUMBSTICK_CLICK, + .touch = VIVE_CONTROLLER_THUMBSTICK_TOUCH}, + + [SURVIVE_BUTTON_SYSTEM] = {.click = VIVE_CONTROLLER_SYSTEM_CLICK, .touch = VIVE_CONTROLLER_SYSTEM_TOUCH}, + + [SURVIVE_BUTTON_MENU] = {.click = VIVE_CONTROLLER_MENU_CLICK, + // only on vive wand without touch + .touch = 0}, + + [SURVIVE_BUTTON_GRIP] = {.click = VIVE_CONTROLLER_SQUEEZE_CLICK, + // only on vive wand without touch + .touch = 0}, + + [SURVIVE_BUTTON_TRIGGER] = {.click = VIVE_CONTROLLER_TRIGGER_CLICK, .touch = VIVE_CONTROLLER_TRIGGER_TOUCH}, +}; + static void survive_controller_get_hand_tracking(struct xrt_device *xdev, enum xrt_input_name name, @@ -415,27 +445,45 @@ survive_controller_get_hand_tracking(struct xrt_device *xdev, thumb_curl = 1.0; } + if (survive->last_inputs[buttons[SURVIVE_BUTTON_TRIGGER].click].value.boolean) { + survive->ctrl.curl[XRT_FINGER_INDEX] = 1.0; + thumb_curl = 1.0; + } + struct u_hand_tracking_curl_values values = {.little = survive->ctrl.curl[XRT_FINGER_LITTLE], .ring = survive->ctrl.curl[XRT_FINGER_RING], .middle = survive->ctrl.curl[XRT_FINGER_MIDDLE], .index = survive->ctrl.curl[XRT_FINGER_INDEX], .thumb = thumb_curl}; + /* The tracked controller position is at the very -z end of the * controller. Move the hand back offset_z meter to the handle center. */ - struct xrt_vec3 static_offset = {.x = 0, .y = 0, .z = 0.11}; + struct xrt_vec3 static_offset = {.x = 0, .y = 0.05, .z = 0.11}; - u_hand_joints_update_curl(&survive->ctrl.hand_tracking, hand, at_timestamp_ns, &values); - struct xrt_pose hand_on_handle_pose; - u_hand_joints_offset_valve_index_controller(hand, &static_offset, &hand_on_handle_pose); struct xrt_space_relation hand_relation; m_relation_history_get(survive->relation_hist, at_timestamp_ns, &hand_relation); - u_hand_joints_set_out_data(&survive->ctrl.hand_tracking, hand, &hand_relation, &hand_on_handle_pose, out_value); + + u_hand_sim_simulate_for_valve_index_knuckles(&values, hand, &hand_relation, out_value); + + struct xrt_pose hand_on_handle_pose; + u_hand_joints_offset_valve_index_controller(hand, &static_offset, &hand_on_handle_pose); + + + struct xrt_relation_chain chain = {0}; + + // out_value->hand_pose = hand_relation; + + m_relation_chain_push_pose(&chain, &hand_on_handle_pose); + m_relation_chain_push_relation(&chain, &hand_relation); + m_relation_chain_resolve(&chain, &out_value->hand_pose); + + // This is the truth - we pose-predicted or interpolated all the way up to `at_timestamp_ns`. *out_timestamp_ns = at_timestamp_ns; @@ -544,33 +592,7 @@ update_axis(struct survive_device *survive, struct Axis *axis, const SurviveSimp return true; } -struct Button -{ - enum input_index click; - enum input_index touch; -}; -struct Button buttons[255] = { - [SURVIVE_BUTTON_A] = {.click = VIVE_CONTROLLER_A_CLICK, .touch = VIVE_CONTROLLER_A_TOUCH}, - [SURVIVE_BUTTON_B] = {.click = VIVE_CONTROLLER_B_CLICK, .touch = VIVE_CONTROLLER_B_TOUCH}, - - [SURVIVE_BUTTON_TRACKPAD] = {.click = VIVE_CONTROLLER_TRACKPAD_CLICK, .touch = VIVE_CONTROLLER_TRACKPAD_TOUCH}, - - [SURVIVE_BUTTON_THUMBSTICK] = {.click = VIVE_CONTROLLER_THUMBSTICK_CLICK, - .touch = VIVE_CONTROLLER_THUMBSTICK_TOUCH}, - - [SURVIVE_BUTTON_SYSTEM] = {.click = VIVE_CONTROLLER_SYSTEM_CLICK, .touch = VIVE_CONTROLLER_SYSTEM_TOUCH}, - - [SURVIVE_BUTTON_MENU] = {.click = VIVE_CONTROLLER_MENU_CLICK, - // only on vive wand without touch - .touch = 0}, - - [SURVIVE_BUTTON_GRIP] = {.click = VIVE_CONTROLLER_SQUEEZE_CLICK, - // only on vive wand without touch - .touch = 0}, - - [SURVIVE_BUTTON_TRIGGER] = {.click = VIVE_CONTROLLER_TRIGGER_CLICK, .touch = VIVE_CONTROLLER_TRIGGER_TOUCH}, -}; static bool update_button(struct survive_device *survive, const struct SurviveSimpleButtonEvent *e, timepoint_ns ts) @@ -1050,8 +1072,6 @@ _create_controller_device(struct survive_system *sys, survive->base.get_hand_tracking = survive_controller_get_hand_tracking; enum xrt_hand hand = idx == SURVIVE_LEFT_CONTROLLER_INDEX ? XRT_HAND_LEFT : XRT_HAND_RIGHT; - u_hand_joints_init_default_set(&survive->ctrl.hand_tracking, hand, XRT_HAND_TRACKING_MODEL_INTRINSIC, - 1.0); survive->base.outputs[0].name = XRT_OUTPUT_NAME_INDEX_HAPTIC; diff --git a/src/xrt/drivers/vive/vive_controller.c b/src/xrt/drivers/vive/vive_controller.c index 24c35964a..f5e1508f9 100644 --- a/src/xrt/drivers/vive/vive_controller.c +++ b/src/xrt/drivers/vive/vive_controller.c @@ -35,6 +35,7 @@ #include "vive.h" #include "vive_protocol.h" #include "vive_controller.h" +#include "util/u_hand_simulation.h" #include #include @@ -336,7 +337,7 @@ vive_controller_get_hand_tracking(struct xrt_device *xdev, .index = (float)d->state.index_finger_trigger / UINT8_MAX, .thumb = thumb_curl}; - u_hand_joints_update_curl(&d->hand_tracking, hand, requested_timestamp_ns, &values); + u_hand_sim_simulate_for_valve_index_knuckles(&values, hand, &out_value->hand_pose, out_value); /* Because IMU is at the very -z end of the controller, the rotation @@ -358,8 +359,6 @@ vive_controller_get_hand_tracking(struct xrt_device *xdev, struct xrt_pose hand_on_handle_pose; u_hand_joints_offset_valve_index_controller(hand, &static_offset, &hand_on_handle_pose); - u_hand_joints_set_out_data(&d->hand_tracking, hand, &controller_relation, &hand_on_handle_pose, out_value); - // This is a lie: currently, no pose-prediction or history is implemented for this driver. *out_timestamp_ns = requested_timestamp_ns; @@ -1105,8 +1104,6 @@ vive_controller_create(struct os_hid_device *controller_hid, enum watchman_gen w enum xrt_hand hand = d->config.variant == CONTROLLER_INDEX_LEFT ? XRT_HAND_LEFT : XRT_HAND_RIGHT; - u_hand_joints_init_default_set(&d->hand_tracking, hand, XRT_HAND_TRACKING_MODEL_INTRINSIC, 1.0); - d->base.binding_profiles = vive_binding_profiles_index; d->base.binding_profile_count = vive_binding_profiles_index_count;