diff --git a/MatplotLeap.py b/MatplotLeap.py index 70eb4ec..59884d6 100644 --- a/MatplotLeap.py +++ b/MatplotLeap.py @@ -287,7 +287,7 @@ def get_rel_bone_points(controller): # Add fingers for finger in fingers: - for joint in range(0,4): + for b in range(0,4): ''' 0 = JOINT_MCP – The metacarpophalangeal joint, or knuckle, of the finger. 1 = JOINT_PIP – The proximal interphalangeal joint of the finger. This joint is the middle joint of a finger. @@ -295,7 +295,8 @@ def get_rel_bone_points(controller): 3 = JOINT_TIP – The tip of the finger. ''' # Transform the finger - transformed_position = hand_transform.transform_point(finger.joint_position(joint)) + bone = finger.bone(b) + transformed_position = hand_transform.transform_point(bone.prev_joint) X.append(transformed_position[0]) Y.append(transformed_position[1]) Z.append(transformed_position[2]) diff --git a/plot_angle_from_rot.py b/plot_angle_from_rot.py index 81a9e64..d8f4a83 100644 --- a/plot_angle_from_rot.py +++ b/plot_angle_from_rot.py @@ -56,6 +56,15 @@ headers+= "," headers = headers[:-2] +# relative origin of fingers for right hand +fingers_rel_orig = [ + [-19.6307, -6.59409, 54.1453], + [-12.1095, 13.6792, 45.7827], + [-0.628944, 14.6466, 42.306], + [10.7102, 11.9239, 41.1872], + [21.4952, 5.17447, 44.1176] +] + def on_close(event): print("Closed Figure") @@ -85,7 +94,7 @@ def get_rotation_matrix(bone): z_basis = basis.z_basis matrix = Leap.Matrix(x_basis, y_basis, z_basis).to_array_3x3() matrix = np.reshape(matrix, newshape=(3, 3)) - print("Basis", matrix) + #print("Basis", matrix) return matrix def get_angles_from_rot(rot_mat): @@ -159,9 +168,7 @@ def get_angles(hand): last_bone_mat = get_rotation_matrix(last_bone) bone_mat = get_rotation_matrix(bone) # Get rotation matrix between bones, change of basis - rot_mat = np.matmul( - bone_mat, last_bone_mat.transpose() - ) + rot_mat = np.matmul(bone_mat, last_bone_mat.transpose()) # Generate euler angles in degrees from rotation matrix bone_angles.append(get_angles_from_rot(rot_mat)) @@ -173,7 +180,7 @@ def animate(i): leapplot.reset_plot(ax) leapplot.reset_plot(ax2) - points = leapplot.get_bone_points(controller) + points = leapplot.get_rel_bone_points(controller) a_points = points if (points is not None): @@ -191,53 +198,57 @@ def animate(i): #print('\r', leap_hand.get_angles(), end='') angles = np.array(get_angles(hand)) - print("angles", angles) - print("angles shape: ", angles.shape) + #print("angles", angles) + #print("angles shape: ", angles.shape) + #hand_transform = Leap.Matrix(hand.basis.x_basis, hand.basis.y_basis, hand.basis.z_basis, hand.palm_position).rigid_inverse() # Turn the angles into points X = [0] Y = [0] Z = [0] for finger in range(0,5): + tmp = fingers_rel_orig[finger]#hand_transform.transform_point(hand.fingers[finger].bone(0).prev_joint) + prev_pos = [-tmp[0],tmp[1],tmp[2]] #[0,0,0] + prev_rot = get_rot_from_angles([0,math.pi,0 if finger != 0 else -math.pi*.6])#np.identity(3) for bone in range(0,4): pitch = angles[finger,bone, 0] yaw = angles[finger,bone, 1] roll = angles[finger,bone, 2] - theta = angles[finger,bone, :] - #theta = [pitch, yaw, roll] - rot_mat = get_rot_from_angles(theta) - # Which basis is this bone defined in??? - bone_assume = np.array([0,20,0]) - new_bone = rot_mat.dot(bone_assume)#.dot(get_rot_from_angles(angles[0, bone, :])) - - # Debugging - if (finger == 1): - if (bone == 1): - print("Pitch degrees",theta[0] * 57.296) - print("Angles", theta) - print("rot_mat ", rot_mat) - print("Det" ,np.linalg.det(rot_mat)) - # Testing time - print("nb", new_bone) + rot_mat = prev_rot.dot(get_rot_from_angles(theta)) + bone_assume = np.array([0,0,hand.fingers[finger].bone(bone).length]) + new_bone = prev_pos + rot_mat.dot(bone_assume) - x = X[finger*3+bone] + new_bone[0] - y = Y[finger*3+bone] + new_bone[1] - z = Z[finger*3+bone] + new_bone[2] - - X.append(x) + # Debugging + # if (finger == 1): + # if (bone == 1): + # print("Pitch degrees",theta[0] * 57.296) + # print("Angles", theta) + # print("rot_mat ", rot_mat) + # print("Det" ,np.linalg.det(rot_mat)) + # # Testing time + # print("nb", new_bone) + + x = new_bone[0] + y = new_bone[1] + z = new_bone[2] + + X.append(-x) Y.append(y) Z.append(z) + prev_pos = new_bone + prev_rot = rot_mat + # Convert to a numpy array a_points = [X, Z, Y] a_points = np.array(a_points) # Creating the 2nd plot - angle_plot = ax2.scatter(a_points[0], a_points[1], a_points[2], alpha=1) + angle_plot = ax2.scatter(a_points[0], a_points[1], a_points[2], s=10, alpha=1) # Plot Angle points leapplot.plot_points(a_points, angle_plot)