Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Hand from joints angles (wip) #4

Open
wants to merge 5 commits into
base: main
Choose a base branch
from
Open

Hand from joints angles (wip) #4

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
5597df1
update get_rel_bone_points in matplotleap for new orion sdk
HyroVitalyProtago Oct 14, 2021
b864d41
wip plot_angle_from_rot
HyroVitalyProtago Oct 14, 2021
e91a25c
update plot_angle_from_rot with relative finger origin
HyroVitalyProtago Oct 14, 2021
e7fd77d
always -x to display left hand
HyroVitalyProtago Oct 14, 2021
510a785
thumb seems rotated around the z axis
HyroVitalyProtago Oct 14, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions MatplotLeap.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -287,15 +287,16 @@ 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.
2 = JOINT_DIP – The distal interphalangeal joint of the finger. This joint is closest to the tip.
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])
Expand Down
69 changes: 40 additions & 29 deletions plot_angle_from_rot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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")

Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -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))
Expand All @@ -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):
Expand All @@ -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)

Expand Down