//Function that phrases in a single finger
private void add_manus_hand(ref manus_hand_t hand, ref manus_hand_raw_t raw_hand, device_type_t which_hand_side, ik_body_t body_side, ik_profile_t my_profile)
{
//Manus Hand Objects
Manus_hand_obj hand_in_use = new Manus_hand_obj();
//Process data for the left hand, including raw data.
Manus.ManusGetHandRaw(session, which_hand_side, out raw_hand);
Manus.ManusGetProfile(session, out my_profile); ///Wrong assumption, it does not provide real-time data.
Manus.ManusGetHand(session, which_hand_side, out hand);
//Manus.ManusGetHand_id(session, 2602524395, which_hand_side, out hand);
Manus.ManusGetBatteryLevel(session, which_hand_side, out bat_value);
//Set Battery level
hand_in_use.set_bat(bat_value);
//Set Arm calculations
add_arm_calc(ref hand, ref body_side, ref my_profile, ref hand_in_use);
//Set the raw_double finger data from manus
add_hand_fingers_raw(ref raw_hand, ref which_hand_side, ref hand_in_use);
//Set manus_profile finger data
add_manus_profile_hands(ref my_profile, ref hand_in_use);
//Set regular Manus hand data
add_hand_fingers(ref hand, ref which_hand_side, ref hand_in_use);
//Add relevant data to the hands array
if (which_hand_side == device_type_t.GLOVE_LEFT)
{
//hands.insert(hands.begin(),hand_in_use); //Left Glove
hands[0] = hand_in_use;
}
else
{
//hands.assign(1, hand_in_use); //Right Glove
hands[1] = hand_in_use;
}
}
//Add finger calculations that are stated to be raw decimal calculations from the Manus SDK
//Phrase the array of raw finger calculations from the Manus SDK.
private void add_hand_fingers_raw(ref manus_hand_raw_t hand, ref device_type_t side, ref Manus_hand_obj manus_hand)
{
List <double> single_hand_array_raw = new List <double>();
bool set_grab = false;
bool set_thumbs_up = false;
for (int h = 0; h < 10; h++)
{
single_hand_array_raw.Add(hand.finger_sensor[h]);
}
int count_grab = 0;
int count_thumbs_up = 0;
//If the finger is compressed beyond .75, add as a compressed finger
foreach (double i in single_hand_array_raw)
{
if (i > .75)
{
count_grab++;
}
//if (i > .95)
// count_thumbs_up++;
}
//if ( single_hand_array_raw[8] < .3 && single_hand_array_raw[9] < .3 && count_thumbs_up >= 7)
//{
// set_thumbs_up = true;
//}
//If more than 8 fingers compressed, set grab.
if (count_grab > 8)
{
set_grab = true;
}
manus_hand.set_grabbing(set_grab);
manus_hand.set_hand_raw(single_hand_array_raw);
}
///***Wrong assumption, it does not provide real-time data***
private void add_manus_profile_hands(ref ik_profile_t my_profile, ref Manus_hand_obj my_hand)
{
List <double> temporary_finger_array = new List <double>(); //Temporary array for doubles to insert into finger.
List <Finger> fingers = new List <Finger>();
//Horrible method of phrasing, but what happens is that a List array is phrased over from the manus array
//Index Finger
for (int c = 0; c < 4; c++)
{
/// temporary_finger_array.Add(my_profile.handProfile.index.bones[c]);
temporary_finger_array.Add(my_profile.handProfile.fingers[0].bones[c]);
// Debug.Log(my_profile.handProfile.fingers[0].bones[c]);
// Debug.Log("Quack");
}
Finger index_bone = new Finger(temporary_finger_array); // Array is inserted into the finger object.
temporary_finger_array.Clear();
fingers.Add(index_bone);//Finger is added into hand.
//Middle Finger
for (int c = 0; c < 4; c++)
{
temporary_finger_array.Add(my_profile.handProfile.fingers[1].bones[c]);
}
Finger middle_bone = new Finger(temporary_finger_array); // Array is inserted into the finger object.
temporary_finger_array.Clear();
fingers.Add(middle_bone);//Finger is added into hand.
//Ring Finger
for (int c = 0; c < 4; c++)
{
temporary_finger_array.Add(my_profile.handProfile.fingers[2].bones[c]);
}
Finger ring_bone = new Finger(temporary_finger_array); // Array is inserted into the finger object.
temporary_finger_array.Clear();
fingers.Add(ring_bone);//Finger is added into hand.
//Pinky Finger
for (int c = 0; c < 4; c++)
{
temporary_finger_array.Add(my_profile.handProfile.fingers[3].bones[c]);
}
Finger pinky_bone = new Finger(temporary_finger_array); // Array is inserted into the finger object.
temporary_finger_array.Clear();
fingers.Add(pinky_bone);//Finger is added into hand.
//Thumb Finger
for (int c = 0; c < 4; c++)
{
temporary_finger_array.Add(my_profile.handProfile.fingers[4].bones[c]);
}
Finger thumb_bone = new Finger(temporary_finger_array); // Array is inserted into the finger object.
temporary_finger_array.Clear();
fingers.Add(thumb_bone); //Finger is added into hand.
my_hand.add_vector_fingers_manus_profile(fingers);
}
//Add vector/quarternion measurements to a finger
private void add_hand_fingers(ref manus_hand_t device, ref device_type_t side, ref Manus_hand_obj manus_hand)
{
List <Finger> single_hand_array = new List <Finger>(); //Finger array
List <pose> temp_finger; //Temporary array for bones.
for (int i = 0; i < 5; i++) //Illiterate through the fingers
{
temp_finger = new List <pose>();
for (int j = 0; j < 5; j++) //Illiterate through the poses.
{
pose temp_pose = new pose(); //Temporary pose format.
temp_pose.rotation = process_quat(device.fingers[i].joints[j].rotation);
temp_pose.translation = process_vector(device.fingers[i].joints[j].translation);
temp_finger.Add(temp_pose); //Add poses to a temporay finger array
}
Finger real_finger = new Finger(temp_finger); //Instantiate a finger class
single_hand_array.Add(real_finger); //Add the finger to the hand
}
//**NEW** Pass imu Data to device.
quat_t[] quarts = device.raw.imu;
Quaternion[] quarts_to_write = new Quaternion[2];
quarts_to_write[0] = process_quat(quarts[0]);
quarts_to_write[1] = process_quat(quarts[1]);
manus_hand.set_imus(quarts_to_write);
manus_hand.add_vector_fingers(single_hand_array); //Add in vector data from manus for each individual bone.
manus_hand.set_wrist(process_quat(device.wrist)); //**NEW** Add in wrist data into the manus_hand_obj
}
//Add the calcuations for the arm calculations on the manus API
private void add_arm_calc(ref manus_hand_t hand, ref ik_body_t body_side, ref ik_profile_t my_profile, ref Manus_hand_obj my_hand)
{
//double arm_calcs[2];
ManusVR.Manus.ManusUpdateIK(session, ref body_side); //Broken, does nothing.
//Hand function to set the profile characteristics of the arm.
my_hand.set_lenghts_arm(my_profile.shoulderLength, my_profile.upperArmLength, my_profile.upperNeckLength, my_profile.lowerArmLength,
my_profile.lowerNeckLength, process_vector(my_profile.upperNeckOffset));
////print_quat(process_quat(body_side.left.lowerArm.rotation));
////print_vector(process_vector(body_side.left.lowerArm.translation));
}
