aux/util: Add u_hand_simulation

Co-authored-by: Moses Turner <moses@collabora.com>
This commit is contained in:
danwillm 2022-08-01 17:17:58 +01:00 committed by Moses Turner
parent 501d2c4fd5
commit 9dc8632b0f
5 changed files with 642 additions and 2 deletions

View file

@ -391,6 +391,26 @@ math_quat_rotate_derivative(const struct xrt_quat *quat, const struct xrt_vec3 *
void
math_quat_slerp(const struct xrt_quat *left, const struct xrt_quat *right, float t, struct xrt_quat *result);
/*!
* Converts a 2D vector to a quaternion
*
* @relates xrt_quat
* @ingroup aux_math
*/
void
math_quat_from_swing(const struct xrt_vec2 *swing, struct xrt_quat *result);
/*!
* Converts a 2D vector and a float to a quaternion
*
* @relates xrt_quat
* @ingorup aux_math
*/
void
math_quat_from_swing_twist(const struct xrt_vec2 *swing, const float twist, struct xrt_quat *result);
/*
*
* Matrix functions

View file

@ -382,6 +382,55 @@ math_quat_slerp(const struct xrt_quat *left, const struct xrt_quat *right, float
map_quat(*result) = l.slerp(t, r);
}
extern "C" void
math_quat_from_swing(const struct xrt_vec2 *swing, struct xrt_quat *result)
{
assert(swing != NULL);
assert(result != NULL);
const float *a0 = &swing->x;
const float *a1 = &swing->y;
const float theta_squared = *a0 * *a0 + *a1 * *a1;
if (theta_squared > 0.f) {
const float theta = sqrt(theta_squared);
const float half_theta = theta * 0.5f;
const float k = sin(half_theta) / theta;
result->w = cos(half_theta);
result->x = *a0 * k;
result->y = *a1 * k;
result->z = 0.f;
} else {
// lim(x->0) (sin(x/2)/x) = 0.5, but sin(0)/0 is undefined, so we need to catch this with a conditional.
const float k(0.5);
result->w = float(1.0);
result->x = *a0 * k;
result->y = *a1 * k;
result->z = float(0);
}
}
extern "C" void
math_quat_from_swing_twist(const struct xrt_vec2 *swing, const float twist, struct xrt_quat *result)
{
assert(swing != NULL);
assert(result != NULL);
struct xrt_quat swing_quat;
struct xrt_quat twist_quat;
struct xrt_vec3 aax_twist;
aax_twist.x = 0.f;
aax_twist.y = 0.f;
aax_twist.z = twist;
math_quat_from_swing(swing, &swing_quat);
math_quat_exp(&aax_twist, &twist_quat);
math_quat_rotate(&swing_quat, &twist_quat, result);
}
/*
*
* Exported matrix functions.

View file

@ -1,7 +1,6 @@
# Copyright 2019-2022, Collabora, Ltd.
# SPDX-License-Identifier: BSL-1.0
configure_file(
"${CMAKE_CURRENT_SOURCE_DIR}/u_git_tag.c.in" "${CMAKE_CURRENT_BINARY_DIR}/u_git_tag.c"
@ONLY
@ -37,6 +36,8 @@ add_library(
u_git_tag.h
u_hand_tracking.c
u_hand_tracking.h
u_hand_simulation.c
u_hand_simulation.h
u_handles.c
u_handles.h
u_hashmap.cpp
@ -99,7 +100,6 @@ add_library(
)
target_link_libraries(aux_util PUBLIC aux-includes xrt-pthreads aux_generated_bindings aux_math)
if(XRT_HAVE_JPEG)
target_link_libraries(aux_util PRIVATE ${JPEG_LIBRARIES})
target_include_directories(aux_util PRIVATE ${JPEG_INCLUDE_DIRS})
@ -124,3 +124,5 @@ endif()
if(ANDROID)
target_link_libraries(aux_util PUBLIC ${ANDROID_LOG_LIBRARY})
endif()
target_include_directories(aux_util PRIVATE ${EIGEN3_INCLUDE_DIR})

View file

@ -0,0 +1,483 @@
// Copyright 2019-2020, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Wrapper around Mercury's parametric hand code, used by Index and OpenGloves to simulate hand tracking.
* @author Christoph Haag <christoph.haag@collabora.com>
* @author Moses Turner <moses@collabora.com>
* @author Daniel Willmott <web@dan-w.com>
* @ingroup aux_util
*/
#include "math/m_mathinclude.h"
#include "util/u_hand_tracking.h"
#include "xrt/xrt_defines.h"
#include "u_hand_simulation.h"
#include "math/m_api.h"
#include "u_trace_marker.h"
#define HAND_SIM_NUM_FINGERS 5
// This is a lie for the thumb; we usually do the hidden metacarpal trick there
#define HAND_SIM_NUM_JOINTS_IN_FINGER 5
#define HAND_SIM_NUM_ORIENTATIONS_IN_FINGER 4
struct translations55
{
struct xrt_vec3 t[HAND_SIM_NUM_FINGERS][HAND_SIM_NUM_JOINTS_IN_FINGER];
};
struct orientations54
{
struct xrt_quat q[HAND_SIM_NUM_FINGERS][HAND_SIM_NUM_ORIENTATIONS_IN_FINGER];
};
#define DEG_TO_RAD(DEG) (DEG * M_PI / 180.)
// For debugging.
#if 0
#include <iostream>
#define assert_quat_length_1(q) \
{ \
const T scale = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]; \
if (abs(scale - T(1.0)) > 0.001) { \
std::cout << "Length bad! " << scale << std::endl; \
assert(false); \
}; \
}
#else
#define assert_quat_length_1(q)
#endif
static void
eval_hand_set_rel_orientations(const struct u_hand_sim_hand *opt, struct orientations54 *rel_orientations)
{
// Thumb MCP hidden orientation
#if 0
Vec2<T> mcp_root_swing;
mcp_root_swing.x = rad<T>((T)(-10));
mcp_root_swing.y = rad<T>((T)(-40));
T mcp_root_twist = rad<T>((T)(-80));
SwingTwistToQuaternion(mcp_root_swing, mcp_root_twist, rel_orientations.q[0][0]);
std::cout << "\n\n\n\nHIDDEN ORIENTATION\n";
std::cout << std::setprecision(100);
std::cout << rel_orientations.q[0][0].w << std::endl;
std::cout << rel_orientations.q[0][0].x << std::endl;
std::cout << rel_orientations.q[0][0].y << std::endl;
std::cout << rel_orientations.q[0][0].z << std::endl;
#else
// This should be exactly equivalent to the above
rel_orientations->q[0][0].w = 0.716990172863006591796875f;
rel_orientations->q[0][0].x = 0.1541481912136077880859375f;
rel_orientations->q[0][0].y = -0.31655871868133544921875f;
rel_orientations->q[0][0].z = -0.6016261577606201171875f;
#endif
// Thumb MCP orientation
math_quat_from_swing_twist(&opt->thumb.metacarpal.swing, //
opt->thumb.metacarpal.twist, //
&rel_orientations->q[0][1]);
// Thumb curls
struct xrt_vec2 thumb_swing0 = {opt->thumb.rotations[0], 0.f};
math_quat_from_swing(&thumb_swing0, &rel_orientations->q[0][2]);
struct xrt_vec2 thumb_swing1 = {opt->thumb.rotations[1], 0.f};
math_quat_from_swing(&thumb_swing1, &rel_orientations->q[0][3]);
// Finger orientations
for (int i = 0; i < 4; i++) {
math_quat_from_swing_twist(&opt->finger[i].metacarpal.swing, //
opt->finger[i].metacarpal.twist, //
&rel_orientations->q[i + 1][0]);
math_quat_from_swing(&opt->finger[i].proximal_swing, //
&rel_orientations->q[i + 1][1]);
struct xrt_vec2 finger_swing0 = {opt->finger[i].rotations[0], 0.f};
math_quat_from_swing(&finger_swing0, &rel_orientations->q[i + 1][2]);
struct xrt_vec2 finger_swing1 = {opt->finger[i].rotations[1], 0.f};
math_quat_from_swing(&finger_swing1, &rel_orientations->q[i + 1][3]);
}
}
static inline void
eval_hand_set_rel_translations(const struct u_hand_sim_hand *opt, struct translations55 *rel_translations)
{
// Basically, we're walking up rel_translations, writing strictly sequentially. Hopefully this is fast.
// Thumb metacarpal translation.
rel_translations->t[0][0] = (struct xrt_vec3){0.33097f, -0.1f, -0.25968f};
// Comes after the invisible joint.
rel_translations->t[0][1] = (struct xrt_vec3){0.f, 0.f, 0.f};
// prox, distal, tip
rel_translations->t[0][2] = (struct xrt_vec3){0.f, 0.f, -0.389626f};
rel_translations->t[0][3] = (struct xrt_vec3){0.f, 0.f, -0.311176f};
rel_translations->t[0][4] = (struct xrt_vec3){0.f, 0.f, -0.232195f};
// What's the best place to put this? Here works, but is there somewhere we could put it where it gets accessed
// faster?
float finger_joint_lengths[4][4] = {
{
-0.66f,
-0.365719f,
-0.231581f,
-0.201790f,
},
{
-0.645f,
-0.404486f,
-0.247749f,
-0.210121f,
},
{
-0.58f,
-0.365639f,
-0.225666f,
-0.187089f,
},
{
-0.52f,
-0.278197f,
-0.176178f,
-0.157566f,
},
};
// Index metacarpal
rel_translations->t[1][0] = (struct xrt_vec3){0.16926f, 0.f, -0.34437f};
// Middle
rel_translations->t[2][0] = (struct xrt_vec3){0.034639f, 0.01f, -0.35573f};
// Ring
rel_translations->t[3][0] = (struct xrt_vec3){-0.063625f, 0.005f, -0.34164f};
// Little
rel_translations->t[4][0] = (struct xrt_vec3){-0.1509f, -0.005f, -0.30373f};
// Index to little finger
for (int finger = 0; finger < 4; finger++) {
for (int i = 0; i < 4; i++) {
int bone = i + 1;
rel_translations->t[finger + 1][bone].x = 0.f;
rel_translations->t[finger + 1][bone].y = 0.f;
rel_translations->t[finger + 1][bone].z = finger_joint_lengths[finger][i];
}
}
}
void
eval_hand_with_orientation(const struct u_hand_sim_hand *opt,
bool is_right,
struct translations55 *translations_absolute,
struct orientations54 *orientations_absolute)
{
XRT_TRACE_MARKER();
struct translations55 rel_translations;
struct orientations54 rel_orientations;
eval_hand_set_rel_orientations(opt, &rel_orientations);
eval_hand_set_rel_translations(opt, &rel_translations);
struct xrt_quat orientation_root = XRT_QUAT_IDENTITY;
// Get each joint's tracking-relative orientation by rotating its parent-relative orientation by the
// tracking-relative orientation of its parent.
for (size_t finger = 0; finger < HAND_SIM_NUM_FINGERS; finger++) {
struct xrt_quat *last_orientation = &orientation_root;
for (size_t bone = 0; bone < HAND_SIM_NUM_ORIENTATIONS_IN_FINGER; bone++) {
struct xrt_quat *rel_orientation = &rel_orientations.q[finger][bone];
struct xrt_quat *out_orientation = &orientations_absolute->q[finger][bone];
math_quat_rotate(last_orientation, rel_orientation, out_orientation);
last_orientation = out_orientation;
}
}
// Get each joint's tracking-relative position by rotating its parent-relative translation by the
// tracking-relative orientation of its parent, then adding that to its parent's tracking-relative position.
struct xrt_vec3 zero = XRT_VEC3_ZERO;
for (size_t finger = 0; finger < HAND_SIM_NUM_FINGERS; finger++) {
const struct xrt_vec3 *last_translation = &zero;
const struct xrt_quat *last_orientation = &orientation_root;
for (size_t bone = 0; bone < HAND_SIM_NUM_JOINTS_IN_FINGER; bone++) {
struct xrt_vec3 *out_translation = &translations_absolute->t[finger][bone];
struct xrt_vec3 *rel_translation = &rel_translations.t[finger][bone];
// rotate and scale
math_quat_rotate_vec3(last_orientation, rel_translation, out_translation);
math_vec3_scalar_mul(opt->hand_size, out_translation);
// If this is a right hand, mirror it.
if (is_right) {
out_translation->x *= -1;
}
out_translation->x += last_translation->x;
out_translation->y += last_translation->y;
out_translation->z += last_translation->z;
// Next iteration, the orientation to rotate by should be the tracking-relative orientation of
// this joint.
// If bone < 4 so we don't go over the end of orientations_absolute. I hope this gets optimized
// out anyway.
if (bone < 4) {
last_orientation = &orientations_absolute->q[finger][bone];
// Ditto for translation
last_translation = out_translation;
}
}
}
}
static inline void
zldtt_ori_right(const struct xrt_quat *orientation, struct xrt_quat *out)
{
struct xrt_quat tmp;
tmp.w = orientation->w;
tmp.x = orientation->x;
tmp.y = orientation->y;
tmp.z = orientation->z;
struct xrt_vec3 x = XRT_VEC3_UNIT_X;
struct xrt_vec3 z = XRT_VEC3_UNIT_Z;
math_quat_rotate_vec3(&tmp, &x, &x);
math_quat_rotate_vec3(&tmp, &z, &z);
// This is a very squashed change-of-basis from left-handed coordinate systems to right-handed coordinate
// systems: you multiply everything by (-1 0 0) then negate the X axis.
x.y *= -1;
x.z *= -1;
z.x *= -1;
math_quat_from_plus_x_z(&x, &z, out);
}
static inline void
zldtt(const struct xrt_vec3 *trans, const struct xrt_quat *orientation, bool is_right, struct xrt_space_relation *out)
{
out->relation_flags = (enum xrt_space_relation_flags)(
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
out->pose.position.x = trans->x;
out->pose.position.y = trans->y;
out->pose.position.z = trans->z;
if (is_right) {
zldtt_ori_right(orientation, &out->pose.orientation);
} else {
out->pose.orientation = *orientation;
}
}
static void
our_eval_to_viz_hand(struct u_hand_sim_hand *opt,
struct translations55 *translations_absolute,
struct orientations54 *orientations_absolute,
bool is_right,
struct xrt_hand_joint_set *out_viz_hand)
{
XRT_TRACE_MARKER();
eval_hand_with_orientation(opt, is_right, translations_absolute, orientations_absolute);
struct xrt_quat final_wrist_orientation = XRT_QUAT_IDENTITY;
int joint_acc_idx = 0;
// Palm.
struct xrt_vec3 palm_position;
palm_position.x = (translations_absolute->t[2][0].x + translations_absolute->t[2][1].x) / 2;
palm_position.y = (translations_absolute->t[2][0].y + translations_absolute->t[2][1].y) / 2;
palm_position.z = (translations_absolute->t[2][0].z + translations_absolute->t[2][1].z) / 2;
struct xrt_quat *palm_orientation = &orientations_absolute->q[2][0];
zldtt(&palm_position, palm_orientation, is_right,
&out_viz_hand->values.hand_joint_set_default[joint_acc_idx++].relation);
// Wrist.
zldtt(&opt->wrist_pose.pose.position, &final_wrist_orientation, is_right,
&out_viz_hand->values.hand_joint_set_default[joint_acc_idx++].relation);
for (int finger = 0; finger < 5; finger++) {
for (int joint = 0; joint < 5; joint++) {
// This one is necessary
if (finger == 0 && joint == 0) {
continue;
}
struct xrt_quat *orientation;
if (joint != 4) {
orientation = &orientations_absolute->q[finger][joint];
} else {
orientation = &orientations_absolute->q[finger][joint - 1];
}
zldtt(&translations_absolute->t[finger][joint], orientation, is_right,
&out_viz_hand->values.hand_joint_set_default[joint_acc_idx++].relation);
}
}
out_viz_hand->is_active = true;
}
static void
hand_sim_hand_init(struct u_hand_sim_hand *out_opt)
{
out_opt->hand_size = 0.095f;
for (int i = 0; i < 4; i++) {
//!@todo needed?
out_opt->finger[i].metacarpal.swing.x = 0.f;
out_opt->finger[i].metacarpal.twist = 0.f;
out_opt->finger[i].proximal_swing.x = DEG_TO_RAD(15);
out_opt->finger[i].rotations[0] = DEG_TO_RAD(-5);
out_opt->finger[i].rotations[1] = DEG_TO_RAD(-5);
}
out_opt->thumb.metacarpal.swing.x = 0.f;
out_opt->thumb.metacarpal.swing.y = 0.f;
out_opt->thumb.metacarpal.twist = 0.f;
out_opt->thumb.rotations[0] = DEG_TO_RAD(10);
out_opt->thumb.rotations[1] = DEG_TO_RAD(10);
out_opt->finger[0].metacarpal.swing.y = -0.19f;
out_opt->finger[1].metacarpal.swing.y = 0.f;
out_opt->finger[2].metacarpal.swing.y = 0.19f;
out_opt->finger[3].metacarpal.swing.y = 0.38f;
out_opt->finger[0].proximal_swing.y = -0.01f;
out_opt->finger[1].proximal_swing.y = 0.f;
out_opt->finger[2].proximal_swing.y = 0.01f;
out_opt->finger[3].proximal_swing.y = 0.02f;
}
void
u_hand_sim_simulate(struct u_hand_sim_hand *hand_ptr, struct xrt_hand_joint_set *out_set)
{
struct translations55 translations;
struct orientations54 orientations;
eval_hand_with_orientation(hand_ptr, hand_ptr->is_right, &translations, &orientations);
our_eval_to_viz_hand(hand_ptr, &translations, &orientations, hand_ptr->is_right, out_set);
u_hand_joints_apply_joint_width(out_set);
out_set->hand_pose = hand_ptr->hand_pose;
out_set->hand_pose.relation_flags = (enum xrt_space_relation_flags)(
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
out_set->is_active = true;
}
void
u_hand_sim_simulate_for_valve_index_knuckles(struct u_hand_tracking_curl_values *values,
bool is_right,
struct xrt_space_relation *root_pose,
struct xrt_hand_joint_set *out_set)
{
struct u_hand_sim_hand hand;
hand.is_right = is_right;
hand_sim_hand_init(&hand);
hand.wrist_pose.pose.position.x = 0.f;
hand.wrist_pose.pose.position.y = 0.f;
hand.wrist_pose.pose.position.z = 0.f;
hand.hand_size = 0.095;
// Thumb
hand.thumb.metacarpal.swing.x += values->thumb * 0.08f;
hand.thumb.metacarpal.swing.y += -0.35f;
hand.thumb.metacarpal.twist = 0;
hand.thumb.rotations[0] += values->thumb * -1.f;
hand.thumb.rotations[1] += values->thumb * -1.f;
// Index finger - this is treated differently on Valve Knuckles controllers so the pinch gesture feels good
float finger_values[4] = {values->index, values->middle, values->little, values->ring};
{
int finger = 0;
float val_turn = finger_values[finger] * -1.1f;
hand.finger[finger].proximal_swing.x = val_turn * 0.9f;
hand.finger[finger].rotations[0] = val_turn;
hand.finger[finger].rotations[1] = val_turn;
}
for (int finger = 1; finger < 4; finger++) {
float val_turn = finger_values[finger] * -1.1f * 1.3f;
hand.finger[finger].proximal_swing.x = val_turn * 1.3f;
hand.finger[finger].rotations[0] = val_turn * 1.0f;
hand.finger[finger].rotations[1] = val_turn * 0.4f;
}
hand.hand_pose = *root_pose;
u_hand_sim_simulate(&hand, out_set);
}
static void
u_hand_sim_apply_generic_finger_transform(const struct u_hand_tracking_finger_value *finger_value,
struct u_hand_sim_finger *out_finger)
{
out_finger->metacarpal.swing.x = finger_value->joint_curls[0] * -1.f;
out_finger->proximal_swing.x = finger_value->joint_curls[1] * -1.f;
out_finger->proximal_swing.y = finger_value->splay;
out_finger->rotations[0] = finger_value->joint_curls[2] * -1.f;
out_finger->rotations[1] = finger_value->joint_curls[3] * -1.f;
}
void
u_hand_sim_simulate_generic(const struct u_hand_tracking_values *values,
bool is_right,
const struct xrt_space_relation *root_pose,
struct xrt_hand_joint_set *out_set)
{
struct u_hand_sim_hand hand;
hand.is_right = is_right;
hand_sim_hand_init(&hand);
hand.wrist_pose.pose.position.x = 0.f;
hand.wrist_pose.pose.position.y = 0.f;
hand.wrist_pose.pose.position.z = 0.f;
hand.hand_size = 0.095;
// Thumb
hand.thumb.metacarpal.swing.x += values->thumb.joint_curls[0] * 0.08f; // curl
hand.thumb.metacarpal.swing.y += values->thumb.splay; // splay
hand.thumb.metacarpal.twist = 0;
hand.thumb.rotations[0] += values->thumb.joint_curls[1] * -1.f;
hand.thumb.rotations[1] += values->thumb.joint_curls[2] * -1.f;
u_hand_sim_apply_generic_finger_transform(&values->little, &hand.finger[3]);
u_hand_sim_apply_generic_finger_transform(&values->ring, &hand.finger[2]);
u_hand_sim_apply_generic_finger_transform(&values->middle, &hand.finger[1]);
u_hand_sim_apply_generic_finger_transform(&values->index, &hand.finger[0]);
hand.hand_pose = *root_pose;
u_hand_sim_simulate(&hand, out_set);
}

View file

@ -0,0 +1,86 @@
// Copyright 2019-2020, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Wrapper around Mercury's parametric hand code, used by Index and OpenGloves to simulate hand tracking.
* @author Christoph Haag <christoph.haag@collabora.com>
* @ingroup aux_util
*/
#pragma once
#include "xrt/xrt_defines.h"
#include "util/u_misc.h"
#include "util/u_hand_tracking.h"
#ifdef __cplusplus
extern "C" {
#endif
/*!
* The hand tracking model being used.
*
* XRT_HAND_TRACKING_MODEL_INTRINSIC for devices that measure hand tracking through sensors, ie gloves and knuckles
* XRT_HAND_TRACKING_MODEL_EXTRINSIC for devices that measure hand tracking through external factors like cameras
*
* @ingroup aux_util
*/
// enum u_hand_tracking_model
// {
// XRT_HAND_TRACKING_MODEL_INTRINSIC,
// XRT_HAND_TRACKING_MODEL_EXTRINSIC,
// };
struct u_hand_sim_metacarpal
{
struct xrt_vec2 swing;
float twist;
};
struct u_hand_sim_finger
{
struct u_hand_sim_metacarpal metacarpal;
struct xrt_vec2 proximal_swing;
// rotation at intermediate joint, then rotation at distal joint
float rotations[2];
};
struct u_hand_sim_thumb
{
struct u_hand_sim_metacarpal metacarpal;
float rotations[2];
};
struct u_hand_sim_hand
{
// Distance from wrist to middle-proximal.
bool is_right;
float hand_size;
struct xrt_space_relation wrist_pose;
struct xrt_space_relation hand_pose;
struct u_hand_sim_thumb thumb;
struct u_hand_sim_finger finger[4];
};
void
u_hand_sim_simulate(struct u_hand_sim_hand *hand, struct xrt_hand_joint_set *out_set);
void
u_hand_sim_simulate_for_valve_index_knuckles(struct u_hand_tracking_curl_values *values,
bool is_right,
struct xrt_space_relation *root_pose,
struct xrt_hand_joint_set *out_set);
void
u_hand_sim_simulate_generic(const struct u_hand_tracking_values *values,
bool is_right,
const struct xrt_space_relation *root_pose,
struct xrt_hand_joint_set *out_set);
#ifdef __cplusplus
}
#endif