mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-15 02:00:22 +00:00
d/vive, d/survive, aux/vive: Fix hand offsets
This commit is contained in:
parent
570a5513be
commit
749d034a14
src/xrt
auxiliary
drivers
|
@ -99,50 +99,3 @@ u_hand_joints_apply_joint_width(struct xrt_hand_joint_set *set)
|
|||
set->values.hand_joint_set_default[XRT_HAND_JOINT_WRIST].radius =
|
||||
.040f * .5f; // Measured my wrist thickness with calipers
|
||||
}
|
||||
|
||||
void
|
||||
u_hand_joints_offset_valve_index_controller(enum xrt_hand hand,
|
||||
const struct xrt_vec3 *static_offset,
|
||||
struct xrt_pose *offset)
|
||||
{
|
||||
/* Controller space origin is at the very tip of the controller,
|
||||
* handle pointing forward at -z.
|
||||
*
|
||||
* Transform joints into controller space by rotating "outwards" around
|
||||
* -z "forward" by -75/75 deg. Then, rotate "forward" around x by 72
|
||||
* deg.
|
||||
*
|
||||
* Then position everything at static_offset..
|
||||
*
|
||||
* Now the hand points "through the strap" like at normal use.
|
||||
*/
|
||||
const struct xrt_vec3 x = XRT_VEC3_UNIT_X;
|
||||
const struct xrt_vec3 y = XRT_VEC3_UNIT_Y;
|
||||
const struct xrt_vec3 negative_z = {0, 0, -1};
|
||||
|
||||
float hand_on_handle_x_rotation = DEG_TO_RAD(-72);
|
||||
float hand_on_handle_y_rotation = 0;
|
||||
float hand_on_handle_z_rotation = 0;
|
||||
if (hand == XRT_HAND_LEFT) {
|
||||
hand_on_handle_z_rotation = DEG_TO_RAD(-75);
|
||||
} else if (hand == XRT_HAND_RIGHT) {
|
||||
hand_on_handle_z_rotation = DEG_TO_RAD(75);
|
||||
}
|
||||
|
||||
|
||||
struct xrt_quat hand_rotation_y = XRT_QUAT_IDENTITY;
|
||||
math_quat_from_angle_vector(hand_on_handle_y_rotation, &y, &hand_rotation_y);
|
||||
|
||||
struct xrt_quat hand_rotation_z = XRT_QUAT_IDENTITY;
|
||||
math_quat_from_angle_vector(hand_on_handle_z_rotation, &negative_z, &hand_rotation_z);
|
||||
|
||||
struct xrt_quat hand_rotation_x = XRT_QUAT_IDENTITY;
|
||||
math_quat_from_angle_vector(hand_on_handle_x_rotation, &x, &hand_rotation_x);
|
||||
|
||||
struct xrt_quat hand_rotation;
|
||||
math_quat_rotate(&hand_rotation_x, &hand_rotation_z, &hand_rotation);
|
||||
|
||||
struct xrt_pose hand_on_handle_pose = {.orientation = hand_rotation, .position = *static_offset};
|
||||
|
||||
*offset = hand_on_handle_pose;
|
||||
}
|
||||
|
|
|
@ -161,17 +161,6 @@ u_hand_joint_is_distal(enum xrt_hand_joint joint);
|
|||
bool
|
||||
u_hand_joint_is_thumb(enum xrt_hand_joint joint);
|
||||
|
||||
/*!
|
||||
* Simple helper function for positioning hands on Valve Index controllers.
|
||||
*
|
||||
* @ingroup aux_util
|
||||
*/
|
||||
void
|
||||
u_hand_joints_offset_valve_index_controller(enum xrt_hand hand,
|
||||
const struct xrt_vec3 *static_offset,
|
||||
struct xrt_pose *offset);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -14,7 +14,7 @@
|
|||
|
||||
#define DEG_TO_RAD(DEG) (DEG * M_PI / 180.)
|
||||
|
||||
void
|
||||
static void
|
||||
vive_poses_apply_right_transform(struct xrt_vec3 *out_transform_position, struct xrt_vec3 *out_transform_rotation)
|
||||
{
|
||||
out_transform_rotation->y *= -1.f;
|
||||
|
@ -23,11 +23,11 @@ vive_poses_apply_right_transform(struct xrt_vec3 *out_transform_position, struct
|
|||
out_transform_position->x *= -1.f;
|
||||
}
|
||||
|
||||
void
|
||||
vive_poses_get_index_offset_transforms(enum xrt_input_name input_name,
|
||||
enum xrt_device_type device_type,
|
||||
struct xrt_vec3 *out_transform_position,
|
||||
struct xrt_vec3 *out_transform_rotation)
|
||||
static void
|
||||
vive_poses_get_index_offset_euler(const enum xrt_input_name input_name,
|
||||
const enum xrt_device_type device_type,
|
||||
struct xrt_vec3 *out_transform_position,
|
||||
struct xrt_vec3 *out_transform_rotation)
|
||||
{
|
||||
switch (input_name) {
|
||||
case XRT_INPUT_INDEX_GRIP_POSE:
|
||||
|
@ -49,16 +49,6 @@ vive_poses_get_index_offset_transforms(enum xrt_input_name input_name,
|
|||
out_transform_rotation->y = DEG_TO_RAD(-5.f);
|
||||
out_transform_rotation->z = 0.f;
|
||||
|
||||
break;
|
||||
case XRT_INPUT_GENERIC_HAND_TRACKING_LEFT:
|
||||
case XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT:
|
||||
out_transform_position->x = -0.05f;
|
||||
out_transform_position->y = -0.015f;
|
||||
out_transform_position->z = 0.13f;
|
||||
|
||||
out_transform_rotation->x = DEG_TO_RAD(14.f);
|
||||
out_transform_rotation->y = DEG_TO_RAD(-0.071f);
|
||||
out_transform_rotation->z = DEG_TO_RAD(10.303);
|
||||
break;
|
||||
default:
|
||||
*out_transform_position = (struct xrt_vec3)XRT_VEC3_ZERO;
|
||||
|
@ -72,23 +62,110 @@ vive_poses_get_index_offset_transforms(enum xrt_input_name input_name,
|
|||
}
|
||||
}
|
||||
|
||||
static void
|
||||
vive_poses_get_index_hand_offset_pose(enum xrt_hand hand, struct xrt_pose *offset)
|
||||
{
|
||||
/* Controller space origin is at the very tip of the controller,
|
||||
* handle pointing forward at -z.
|
||||
*
|
||||
* Transform joints into controller space by rotating "outwards" around
|
||||
* -z "forward" by -75/75 deg. Then, rotate "forward" around x by 72
|
||||
* deg.
|
||||
*
|
||||
* Then position everything at static_offset..
|
||||
*
|
||||
* Now the hand points "through the strap" like at normal use.
|
||||
*/
|
||||
struct xrt_vec3 offset_position = {.x = 0, .y = 0.05, .z = 0.11};
|
||||
#if 0
|
||||
const struct xrt_vec3 x = XRT_VEC3_UNIT_X;
|
||||
const struct xrt_vec3 y = XRT_VEC3_UNIT_Y;
|
||||
const struct xrt_vec3 negative_z = {0, 0, -1};
|
||||
|
||||
float hand_on_handle_x_rotation = DEG_TO_RAD(-72);
|
||||
float hand_on_handle_y_rotation = 0;
|
||||
float hand_on_handle_z_rotation = 0;
|
||||
if (hand == XRT_HAND_LEFT) {
|
||||
hand_on_handle_z_rotation = DEG_TO_RAD(-75);
|
||||
} else if (hand == XRT_HAND_RIGHT) {
|
||||
hand_on_handle_z_rotation = DEG_TO_RAD(75);
|
||||
}
|
||||
|
||||
|
||||
struct xrt_quat hand_rotation_y = XRT_QUAT_IDENTITY;
|
||||
math_quat_from_angle_vector(hand_on_handle_y_rotation, &y, &hand_rotation_y);
|
||||
|
||||
struct xrt_quat hand_rotation_z = XRT_QUAT_IDENTITY;
|
||||
math_quat_from_angle_vector(hand_on_handle_z_rotation, &negative_z, &hand_rotation_z);
|
||||
|
||||
struct xrt_quat hand_rotation_x = XRT_QUAT_IDENTITY;
|
||||
math_quat_from_angle_vector(hand_on_handle_x_rotation, &x, &hand_rotation_x);
|
||||
|
||||
struct xrt_quat hand_rotation;
|
||||
math_quat_rotate(&hand_rotation_x, &hand_rotation_z, &hand_rotation);
|
||||
|
||||
struct xrt_pose hand_on_handle_pose = {.orientation = hand_rotation, .position = offset_position};
|
||||
|
||||
printf("%d %f %f %f %f %f %f %f\n", hand, hand_on_handle_pose.position.x, hand_on_handle_pose.position.y, hand_on_handle_pose.position.z, hand_on_handle_pose.orientation.w, \
|
||||
hand_on_handle_pose.orientation.x, hand_on_handle_pose.orientation.y, hand_on_handle_pose.orientation.z);
|
||||
|
||||
*offset = hand_on_handle_pose;
|
||||
#else
|
||||
switch (hand) {
|
||||
case XRT_HAND_LEFT: {
|
||||
*offset = (struct xrt_pose){
|
||||
.orientation = {.w = 0.641836, .x = -0.466321, .y = 0.357821, .z = 0.492498},
|
||||
.position = offset_position,
|
||||
};
|
||||
break;
|
||||
}
|
||||
|
||||
case XRT_HAND_RIGHT: {
|
||||
*offset = (struct xrt_pose){
|
||||
.orientation = {.w = 0.641836, .x = -0.466321, .y = -0.357821, .z = -0.492498},
|
||||
.position = offset_position,
|
||||
};
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
static void
|
||||
vive_poses_get_index_offset_pose(const enum xrt_input_name input_name,
|
||||
const enum xrt_device_type device_type,
|
||||
struct xrt_pose *out_offset_pose)
|
||||
{
|
||||
switch (input_name) {
|
||||
case XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT:
|
||||
vive_poses_get_index_hand_offset_pose(XRT_HAND_RIGHT, out_offset_pose);
|
||||
return;
|
||||
case XRT_INPUT_GENERIC_HAND_TRACKING_LEFT:
|
||||
vive_poses_get_index_hand_offset_pose(XRT_HAND_LEFT, out_offset_pose);
|
||||
return;
|
||||
default: break; // Go to code below.
|
||||
}
|
||||
|
||||
// Note that XRT_INPUT_GENERIC_TRACKER_POSE goes down this path
|
||||
struct xrt_vec3 transform_position = XRT_VEC3_ZERO;
|
||||
struct xrt_vec3 transform_rotation = XRT_VEC3_ZERO;
|
||||
vive_poses_get_index_offset_euler(input_name, device_type, &transform_position, &transform_rotation);
|
||||
math_quat_from_euler_angles(&transform_rotation, &out_offset_pose->orientation);
|
||||
out_offset_pose->position = transform_position;
|
||||
}
|
||||
|
||||
void
|
||||
vive_poses_get_pose_offset(enum xrt_device_name device_name,
|
||||
enum xrt_device_type device_type,
|
||||
enum xrt_input_name input_name,
|
||||
struct xrt_pose *out_offset_pose)
|
||||
{
|
||||
struct xrt_vec3 transform_position = XRT_VEC3_ZERO;
|
||||
struct xrt_vec3 transform_rotation = XRT_VEC3_ZERO;
|
||||
|
||||
switch (device_name) {
|
||||
case XRT_DEVICE_INDEX_CONTROLLER:
|
||||
vive_poses_get_index_offset_transforms(input_name, device_type, &transform_position,
|
||||
&transform_rotation);
|
||||
vive_poses_get_index_offset_pose(input_name, device_type, out_offset_pose);
|
||||
break;
|
||||
default: break;
|
||||
default: *out_offset_pose = (struct xrt_pose)XRT_POSE_IDENTITY;
|
||||
}
|
||||
|
||||
math_quat_from_euler_angles(&transform_rotation, &out_offset_pose->orientation);
|
||||
out_offset_pose->position = transform_position;
|
||||
}
|
||||
|
|
|
@ -12,6 +12,13 @@
|
|||
#include "xrt/xrt_defines.h"
|
||||
#include "vive_config.h"
|
||||
|
||||
/*!
|
||||
* Returns the offset from a controller's IMU to the aim pose, grip pose or wrist pose (P_imu_{aim,grip,wrist}).
|
||||
*
|
||||
* Return a non-identity pose on
|
||||
* Returns XRT_POSE_IDENTITY on XRT_INPUT_GENERIC_TRACKER_POSE.
|
||||
*/
|
||||
|
||||
void
|
||||
vive_poses_get_pose_offset(enum xrt_device_name device_name,
|
||||
enum xrt_device_type device_type,
|
||||
|
|
|
@ -341,6 +341,7 @@ survive_device_get_tracked_pose(struct xrt_device *xdev,
|
|||
return;
|
||||
}
|
||||
|
||||
// We're pretty sure libsurvive is giving us the IMU pose here, so this works.
|
||||
struct xrt_pose pose_offset = XRT_POSE_IDENTITY;
|
||||
vive_poses_get_pose_offset(survive->base.name, survive->base.device_type, name, &pose_offset);
|
||||
|
||||
|
@ -476,11 +477,6 @@ survive_controller_get_hand_tracking(struct xrt_device *xdev,
|
|||
.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.05, .z = 0.11};
|
||||
|
||||
struct xrt_space_relation hand_relation;
|
||||
|
||||
m_relation_history_get(survive->relation_hist, at_timestamp_ns, &hand_relation);
|
||||
|
@ -488,18 +484,13 @@ survive_controller_get_hand_tracking(struct xrt_device *xdev,
|
|||
|
||||
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;
|
||||
|
||||
// We're pretty sure libsurvive is giving us the IMU pose here, so this works.
|
||||
struct xrt_pose pose_offset = XRT_POSE_IDENTITY;
|
||||
vive_poses_get_pose_offset(survive->base.name, survive->base.device_type, name, &pose_offset);
|
||||
|
||||
m_relation_chain_push_pose(&chain, &hand_on_handle_pose);
|
||||
m_relation_chain_push_pose(&chain, &pose_offset);
|
||||
m_relation_chain_push_relation(&chain, &hand_relation);
|
||||
m_relation_chain_resolve(&chain, &out_value->hand_pose);
|
||||
|
|
|
@ -32,6 +32,7 @@
|
|||
|
||||
#include "vive/vive_config.h"
|
||||
#include "vive/vive_bindings.h"
|
||||
#include "vive/vive_poses.h"
|
||||
|
||||
#include "vive.h"
|
||||
#include "vive_protocol.h"
|
||||
|
@ -305,6 +306,25 @@ _update_tracker_inputs(struct xrt_device *xdev)
|
|||
// Nothing to do here as the device does not send button reports.
|
||||
}
|
||||
|
||||
static void
|
||||
predict_pose(struct vive_controller_device *d, uint64_t at_timestamp_ns, struct xrt_space_relation *out_relation)
|
||||
{
|
||||
timepoint_ns prediction_ns = at_timestamp_ns - d->imu.ts_received_ns;
|
||||
double prediction_s = time_ns_to_s(prediction_ns);
|
||||
|
||||
timepoint_ns monotonic_now_ns = os_monotonic_get_ns();
|
||||
timepoint_ns remaining_ns = at_timestamp_ns - monotonic_now_ns;
|
||||
VIVE_TRACE(d, "dev %s At %ldns: Pose requested for +%ldns (%ldns), predicting %ldns", d->base.str,
|
||||
monotonic_now_ns, remaining_ns, at_timestamp_ns, prediction_ns);
|
||||
|
||||
//! @todo integrate position here
|
||||
struct xrt_space_relation relation = {0};
|
||||
relation.pose.orientation = d->rot_filtered;
|
||||
relation.relation_flags = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT;
|
||||
|
||||
m_predict_relation(&relation, prediction_s, out_relation);
|
||||
}
|
||||
|
||||
static void
|
||||
vive_controller_get_hand_tracking(struct xrt_device *xdev,
|
||||
enum xrt_input_name name,
|
||||
|
@ -340,58 +360,31 @@ vive_controller_get_hand_tracking(struct xrt_device *xdev,
|
|||
.thumb = thumb_curl,
|
||||
};
|
||||
|
||||
struct xrt_space_relation ident;
|
||||
m_space_relation_ident(&ident);
|
||||
u_hand_sim_simulate_for_valve_index_knuckles(&values, hand, &ident, out_value);
|
||||
struct xrt_space_relation hand_relation;
|
||||
|
||||
predict_pose(d, requested_timestamp_ns, &hand_relation);
|
||||
|
||||
u_hand_sim_simulate_for_valve_index_knuckles(&values, hand, &hand_relation, out_value);
|
||||
|
||||
struct xrt_relation_chain chain = {0};
|
||||
|
||||
|
||||
/* Because IMU is at the very -z end of the controller, the rotation
|
||||
* pivot point is there too. By offsetting the IMU pose by this z value
|
||||
* we move the pivot point of the hand.
|
||||
* This only makes sense with 3dof.
|
||||
*/
|
||||
struct xrt_pose pose_offset = XRT_POSE_IDENTITY;
|
||||
vive_poses_get_pose_offset(d->base.name, d->base.device_type, name, &pose_offset);
|
||||
pose_offset.position = (struct xrt_vec3)XRT_VEC3_ZERO;
|
||||
|
||||
#if 0
|
||||
float pivot_offset_z = 0.15;
|
||||
m_relation_chain_push_pose(&chain, &pose_offset);
|
||||
m_relation_chain_push_relation(&chain, &hand_relation);
|
||||
m_relation_chain_resolve(&chain, &out_value->hand_pose);
|
||||
|
||||
struct xrt_space_relation controller_relation = {
|
||||
.pose = {.orientation = d->rot_filtered, .position = {0, 0, pivot_offset_z}},
|
||||
};
|
||||
controller_relation.relation_flags = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
|
||||
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
|
||||
XRT_SPACE_RELATION_POSITION_VALID_BIT;
|
||||
#endif
|
||||
|
||||
struct xrt_vec3 static_offset = {0, 0, 0};
|
||||
|
||||
struct xrt_pose hand_on_handle_pose;
|
||||
u_hand_joints_offset_valve_index_controller(hand, &static_offset, &hand_on_handle_pose);
|
||||
|
||||
// This is a lie: currently, no pose-prediction or history is implemented for this driver.
|
||||
// This is the truth - we pose-predicted or interpolated all the way up to `at_timestamp_ns`.
|
||||
*out_timestamp_ns = requested_timestamp_ns;
|
||||
|
||||
// This is a lie - apparently libsurvive doesn't report controller tracked/untracked state, so just say that the
|
||||
// hand is being tracked
|
||||
out_value->is_active = true;
|
||||
}
|
||||
|
||||
static void
|
||||
predict_pose(struct vive_controller_device *d, uint64_t at_timestamp_ns, struct xrt_space_relation *out_relation)
|
||||
{
|
||||
timepoint_ns prediction_ns = at_timestamp_ns - d->imu.ts_received_ns;
|
||||
double prediction_s = time_ns_to_s(prediction_ns);
|
||||
|
||||
timepoint_ns monotonic_now_ns = os_monotonic_get_ns();
|
||||
timepoint_ns remaining_ns = at_timestamp_ns - monotonic_now_ns;
|
||||
VIVE_TRACE(d, "dev %s At %ldns: Pose requested for +%ldns (%ldns), predicting %ldns", d->base.str,
|
||||
monotonic_now_ns, remaining_ns, at_timestamp_ns, prediction_ns);
|
||||
|
||||
//! @todo integrate position here
|
||||
struct xrt_space_relation relation = {0};
|
||||
relation.pose.orientation = d->rot_filtered;
|
||||
relation.relation_flags = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT;
|
||||
|
||||
m_predict_relation(&relation, prediction_s, out_relation);
|
||||
}
|
||||
|
||||
static void
|
||||
vive_controller_device_get_tracked_pose(struct xrt_device *xdev,
|
||||
enum xrt_input_name name,
|
||||
|
|
Loading…
Reference in a new issue