From 5597df13c7fc92445a8869062b5b3b101c8f2ad8 Mon Sep 17 00:00:00 2001 From: Ludovic Date: Thu, 14 Oct 2021 16:56:03 +0200 Subject: [PATCH 1/5] update get_rel_bone_points in matplotleap for new orion sdk --- MatplotLeap.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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]) From b864d41b9cfe9da5e7063a3f70248358af6fa4d4 Mon Sep 17 00:00:00 2001 From: Ludovic Date: Thu, 14 Oct 2021 16:57:04 +0200 Subject: [PATCH 2/5] wip plot_angle_from_rot --- plot_angle_from_rot.py | 50 +++++++++++++++++++++--------------------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/plot_angle_from_rot.py b/plot_angle_from_rot.py index 81a9e64..019c228 100644 --- a/plot_angle_from_rot.py +++ b/plot_angle_from_rot.py @@ -85,7 +85,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 +159,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 +171,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,47 +189,49 @@ 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) # Turn the angles into points X = [0] Y = [0] Z = [0] for finger in range(0,5): + prev_pos = [0,0,0] + prev_rot = 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, :])) + 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) # 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) + # 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 = X[finger*3+bone] + new_bone[0] - y = Y[finger*3+bone] + new_bone[1] - z = Z[finger*3+bone] + new_bone[2] + 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) From e91a25c98ab4b675a47bfcd1858aeeca2b60c678 Mon Sep 17 00:00:00 2001 From: Ludovic Date: Thu, 14 Oct 2021 17:36:48 +0200 Subject: [PATCH 3/5] update plot_angle_from_rot with relative finger origin - TODO work on thumb rotation --- plot_angle_from_rot.py | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/plot_angle_from_rot.py b/plot_angle_from_rot.py index 019c228..a108b2c 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") @@ -192,13 +201,16 @@ def animate(i): #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): - prev_pos = [0,0,0] - prev_rot = np.identity(3) + tmp = fingers_rel_orig[finger]#hand_transform.transform_point(hand.fingers[finger].bone(0).prev_joint) + prev_pos = [tmp[0] * (-1 if hand.is_left else 1),tmp[1],tmp[2]] #[0,0,0] + prev_rot = get_rot_from_angles([0,math.pi,0])#np.identity(3) for bone in range(0,4): pitch = angles[finger,bone, 0] yaw = angles[finger,bone, 1] @@ -220,12 +232,11 @@ def animate(i): # # Testing time # print("nb", new_bone) - x = new_bone[0] y = new_bone[1] z = new_bone[2] - X.append(x) + X.append(x * (-1 if hand.is_left else 1)) Y.append(y) Z.append(z) @@ -237,7 +248,7 @@ def animate(i): 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) From e7fd77d83ff8d59cafa5fed0513755f9ad3b9948 Mon Sep 17 00:00:00 2001 From: Ludovic Date: Thu, 14 Oct 2021 17:48:53 +0200 Subject: [PATCH 4/5] always -x to display left hand --- plot_angle_from_rot.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plot_angle_from_rot.py b/plot_angle_from_rot.py index a108b2c..62a51ac 100644 --- a/plot_angle_from_rot.py +++ b/plot_angle_from_rot.py @@ -209,7 +209,7 @@ def animate(i): 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] * (-1 if hand.is_left else 1),tmp[1],tmp[2]] #[0,0,0] + prev_pos = [-tmp[0],tmp[1],tmp[2]] #[0,0,0] prev_rot = get_rot_from_angles([0,math.pi,0])#np.identity(3) for bone in range(0,4): pitch = angles[finger,bone, 0] @@ -236,7 +236,7 @@ def animate(i): y = new_bone[1] z = new_bone[2] - X.append(x * (-1 if hand.is_left else 1)) + X.append(-x) Y.append(y) Z.append(z) From 510a785a590d1170e31da3bbbe07f6bd852eef31 Mon Sep 17 00:00:00 2001 From: Ludovic Date: Thu, 14 Oct 2021 19:07:58 +0200 Subject: [PATCH 5/5] thumb seems rotated around the z axis --- plot_angle_from_rot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plot_angle_from_rot.py b/plot_angle_from_rot.py index 62a51ac..d8f4a83 100644 --- a/plot_angle_from_rot.py +++ b/plot_angle_from_rot.py @@ -210,7 +210,7 @@ def animate(i): 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])#np.identity(3) + 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]