Skip to content
GitLab
Commit 27ebcf08 authored by Moses Turner's avatar Moses Turner
Browse files

d/ulv2: undo hand origin stuff and change space graph to make hands track properly

parent 735c50a2
Pipeline #255032 failed with stages
in 1 minute and 19 seconds
Hide whitespace changes
Inline Side-by-side
src/xrt/drivers/ultraleap_v2/ulv2_driver.cpp
View file @ 27ebcf08
......@@ -87,38 +87,38 @@ relation_from_leap(struct xrt_space_relation *relation, Leap::Vector pp, Leap::M
float ymul = -1/1000.0;
float zmul = -1/1000.0;
mat.xBasis.x *= xmul;
mat.xBasis.y *= ymul;
mat.xBasis.z *= zmul;
// mat.xBasis.x *= xmul;
// mat.xBasis.y *= ymul;
// mat.xBasis.z *= zmul;
mat.yBasis.x *= xmul;
mat.yBasis.y *= ymul;
mat.yBasis.z *= zmul;
// mat.yBasis.x *= xmul;
// mat.yBasis.y *= ymul;
// mat.yBasis.z *= zmul;
mat.zBasis.x *= xmul;
mat.zBasis.y *= ymul;
mat.zBasis.z *= zmul;
// mat.zBasis.x *= xmul;
// mat.zBasis.y *= ymul;
// mat.zBasis.z *= zmul;
struct xrt_matrix_3x3 boo =
{ mat.xBasis.x,mat.xBasis.y,mat.xBasis.z,
mat.yBasis.x,mat.yBasis.y,mat.yBasis.z,
mat.zBasis.x,mat.zBasis.y,mat.zBasis.z
};
// struct xrt_matrix_3x3 boo =
// { mat.xBasis.x,mat.xBasis.y,mat.xBasis.z,
// mat.yBasis.x,mat.yBasis.y,mat.yBasis.z,
// mat.zBasis.x,mat.zBasis.y,mat.zBasis.z
// };
struct xrt_quat ori;
// struct xrt_quat ori;
math_quat_from_matrix_3x3(&boo, &ori);
// math_quat_from_matrix_3x3(&boo, &ori);
relation->pose.position.x = xmul;
relation->pose.position.y = ymul;
relation->pose.position.z = zmul;
relation->pose.orientation.x = ori.x;
relation->pose.orientation.y = ori.y;
relation->pose.orientation.z = ori.z;
relation->pose.orientation.w = ori.w;
relation->pose.position.x = pp.x*xmul;
relation->pose.position.y = pp.z*ymul;
relation->pose.position.z = pp.y*zmul;
relation->pose.orientation.x = 0;
relation->pose.orientation.y = 0;
relation->pose.orientation.z = 0;
relation->pose.orientation.w = 1;
relation->relation_flags = valid_flags;
}
......@@ -164,14 +164,14 @@ void *heres_the_pthread(void *ptr_to_xdev) {
}
// The center position of the palm in millimeters from the Leap Motion Controller origin.
relation_from_leap(&put_in_hand->hand_pose, hand.palmPosition(), Leap::Matrix());
//relation_from_leap(&put_in_hand->hand_pose, hand.palmPosition(), Leap::Matrix());
//! @todo: Palm at 0,0,0 in "hand coordinates"?
Leap::Vector zero_vec = {0,0,0};
zldtt(&put_in_hand->values.hand_joint_set_default[XRT_HAND_JOINT_PALM],zero_vec,
Leap::Matrix(), 50);
zldtt(&put_in_hand->values.hand_joint_set_default[XRT_HAND_JOINT_WRIST],zero_vec,
Leap::Matrix(), 50);
// //! @todo: Palm at 0,0,0 in "hand coordinates"?
// Leap::Vector zero_vec = {0,0,0};
zldtt(&put_in_hand->values.hand_joint_set_default[XRT_HAND_JOINT_PALM],hand.palmPosition(), Leap::Matrix(), 50);
zldtt(&put_in_hand->values.hand_joint_set_default[XRT_HAND_JOINT_WRIST],hand.wristPosition(), Leap::Matrix(), 50);
// moses says: the above does weird stuff. currently, hand_pose is just gonna be the LMC pose
const Leap::FingerList fingers = hand.fingers();
......@@ -219,39 +219,39 @@ void *heres_the_pthread(void *ptr_to_xdev) {
nextJ(ld),fb(ld).basis(),fb(ld).width());
break;
case Leap::Finger::Type::TYPE_MIDDLE:
zldtt(e(XRT_HAND_JOINT_INDEX_METACARPAL),
zldtt(e(XRT_HAND_JOINT_MIDDLE_METACARPAL),
prevJ(lm),fb(lm).basis(),fb(lm).width());
zldtt(e(XRT_HAND_JOINT_INDEX_PROXIMAL),
zldtt(e(XRT_HAND_JOINT_MIDDLE_PROXIMAL),
prevJ(lp),fb(lp).basis(),fb(lm).width());
zldtt(e(XRT_HAND_JOINT_INDEX_INTERMEDIATE),
zldtt(e(XRT_HAND_JOINT_MIDDLE_INTERMEDIATE),
prevJ(li),fb(li).basis(),fb(lp).width());
zldtt(e(XRT_HAND_JOINT_INDEX_DISTAL),
zldtt(e(XRT_HAND_JOINT_MIDDLE_DISTAL),
prevJ(ld),fb(ld).basis(),fb(li).width());
zldtt(e(XRT_HAND_JOINT_INDEX_TIP),
zldtt(e(XRT_HAND_JOINT_MIDDLE_TIP),
nextJ(ld),fb(ld).basis(),fb(ld).width());
break;
case Leap::Finger::Type::TYPE_RING:
zldtt(e(XRT_HAND_JOINT_INDEX_METACARPAL),
zldtt(e(XRT_HAND_JOINT_RING_METACARPAL),
prevJ(lm),fb(lm).basis(),fb(lm).width());
zldtt(e(XRT_HAND_JOINT_INDEX_PROXIMAL),
zldtt(e(XRT_HAND_JOINT_RING_PROXIMAL),
prevJ(lp),fb(lp).basis(),fb(lm).width());
zldtt(e(XRT_HAND_JOINT_INDEX_INTERMEDIATE),
zldtt(e(XRT_HAND_JOINT_RING_INTERMEDIATE),
prevJ(li),fb(li).basis(),fb(lp).width());
zldtt(e(XRT_HAND_JOINT_INDEX_DISTAL),
zldtt(e(XRT_HAND_JOINT_RING_DISTAL),
prevJ(ld),fb(ld).basis(),fb(li).width());
zldtt(e(XRT_HAND_JOINT_INDEX_TIP),
zldtt(e(XRT_HAND_JOINT_RING_TIP),
nextJ(ld),fb(ld).basis(),fb(ld).width());
break;
case Leap::Finger::Type::TYPE_PINKY:
zldtt(e(XRT_HAND_JOINT_INDEX_METACARPAL),
zldtt(e(XRT_HAND_JOINT_LITTLE_METACARPAL),
prevJ(lm),fb(lm).basis(),fb(lm).width());
zldtt(e(XRT_HAND_JOINT_INDEX_PROXIMAL),
zldtt(e(XRT_HAND_JOINT_LITTLE_PROXIMAL),
prevJ(lp),fb(lp).basis(),fb(lm).width());
zldtt(e(XRT_HAND_JOINT_INDEX_INTERMEDIATE),
zldtt(e(XRT_HAND_JOINT_LITTLE_INTERMEDIATE),
prevJ(li),fb(li).basis(),fb(lp).width());
zldtt(e(XRT_HAND_JOINT_INDEX_DISTAL),
zldtt(e(XRT_HAND_JOINT_LITTLE_DISTAL),
prevJ(ld),fb(ld).basis(),fb(li).width());
zldtt(e(XRT_HAND_JOINT_INDEX_TIP),
zldtt(e(XRT_HAND_JOINT_LITTLE_TIP),
nextJ(ld),fb(ld).basis(),fb(ld).width());
break;
break;
......@@ -323,7 +323,6 @@ ulv2_device_get_hand_tracking(struct xrt_device *xdev,
pthread_mutex_lock(&lock_yours);
memcpy(out_value,&your_right_hand,sizeof(struct xrt_hand_joint_set));
hand_valid = your_right_hand_exists;
pthread_mutex_unlock(&lock_yours);
} else {
ULV2_ERROR(ulv2d, "unknown input name for hand tracker");
......@@ -339,18 +338,21 @@ ulv2_device_get_hand_tracking(struct xrt_device *xdev,
if(hand_valid) {
out_value->isActive = true;
struct xrt_space_relation ns_to_leap_relation = {.relation_flags = valid_flags, .pose = {{0,0,0,1}, {0,.0842,-.0705}}, .linear_velocity= {0,0,0}, .angular_velocity = {0,0,0}};
struct xrt_space_relation ns_to_leap_relation = {.relation_flags = valid_flags, .pose = {{0,0,0,1}, {0,.0842,-.09}}, .linear_velocity= {0,0,0}, .angular_velocity = {0,0,0}};
struct xrt_space_graph xsg = {};
m_space_graph_add_relation(&xsg, &out_value->hand_pose);
//m_space_graph_add_relation(&xsg, &out_value->hand_pose);
struct xrt_space_relation ns_relation;
if (tracker_device != NULL){ // WILL be null if the realsense is not plugged in
// if (tracker_device != NULL){ // WILL be null if the realsense is not plugged in
// printf("notnull\n");
tracker_device->get_tracked_pose(tracker_device,XRT_INPUT_GENERIC_HEAD_POSE,at_timestamp_ns,&ns_relation);
m_space_graph_add_relation(&xsg, &ns_relation);}
m_space_graph_add_relation(&xsg, &ns_to_leap_relation);
m_space_graph_add_relation(&xsg, &ns_relation);
// }
m_space_graph_resolve(&xsg, &out_value->hand_pose);
struct xrt_space_relation res;
......@@ -359,6 +361,9 @@ ulv2_device_get_hand_tracking(struct xrt_device *xdev,
out_value->hand_origin = res;
// printf("hidf "); // printf_pose segfaults unless I print something in front of it??
// printf_pose(out_value->hand_origin.pose);
printf("hidf "); // printf_pose segfaults unless I print something in front of it??
printf_pose(ns_relation.pose);
printf_pose(out_value->hand_pose.pose);
printf("hidf "); // printf_pose segfaults unless I print something in front of it??
printf_pose(out_value->hand_origin.pose);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment