|
a |
|
b/app/config/AnybodyFirstFrame.py |
|
|
1 |
import json |
|
|
2 |
import numpy as np |
|
|
3 |
|
|
|
4 |
|
|
|
5 |
# rotation matrix from anybody to LeapMotion |
|
|
6 |
ROTATION_KS = np.array([[0, 0, 1], |
|
|
7 |
[0, 1, 0], |
|
|
8 |
[-1, 0, 0]]) |
|
|
9 |
|
|
|
10 |
ROTATION_FINGER = np.array([[0, -1, 0], |
|
|
11 |
[-1, 0, 0], |
|
|
12 |
[0, 0, -1]]) |
|
|
13 |
|
|
|
14 |
ROTATION_THUMB = np.array([[0.9497, 0.1712, -0.2624], |
|
|
15 |
[-0.0351, -0.7742, -0.6320], |
|
|
16 |
[-0.3113, 0.6094, -0.7292]]) |
|
|
17 |
|
|
|
18 |
ROTATION_HAND = np.array([[-1, -0.2, -0.0], |
|
|
19 |
[0.2, -0.5, 0.9], |
|
|
20 |
[-0.2, 0.88, 0.5]]) |
|
|
21 |
|
|
|
22 |
ROTATION_ELBOW = np.array([[-1, 0, 0], |
|
|
23 |
[0, -0.3, 1], |
|
|
24 |
[0, 1, 0.3]]) |
|
|
25 |
|
|
|
26 |
class AnybodyFirstFrame: |
|
|
27 |
|
|
|
28 |
def __init__(self, update_values=False): |
|
|
29 |
self.joint_values = None |
|
|
30 |
if not update_values: |
|
|
31 |
self.load('config/anybody_joint_values_waagerecht.json') |
|
|
32 |
|
|
|
33 |
def load(self, filename): |
|
|
34 |
with open(filename) as o: |
|
|
35 |
self.joint_values = json.load(o) |
|
|
36 |
|
|
|
37 |
def get_basis(self, joint_name): |
|
|
38 |
anybody_basis = self.joint_values[joint_name]['Axes0'] |
|
|
39 |
# print("joint: {}, basis: \n{}".format(joint_name, np.matmul(ROTATION_BASIS, |
|
|
40 |
# np.matmul(ROTATION_KS, np.array(anybody_basis))))) |
|
|
41 |
if joint_name in ('RightElbow'): |
|
|
42 |
return np.matmul(ROTATION_ELBOW, np.transpose(np.array(anybody_basis))) |
|
|
43 |
if joint_name in ('RightHand'): |
|
|
44 |
return np.matmul(ROTATION_HAND, np.transpose(np.array(anybody_basis))) |
|
|
45 |
if 'RightHandThumb' in joint_name: |
|
|
46 |
return np.matmul(ROTATION_THUMB, np.transpose(np.array(anybody_basis))) |
|
|
47 |
return np.matmul(ROTATION_FINGER, np.transpose(np.array(anybody_basis))) |
|
|
48 |
|
|
|
49 |
def get_position(self, joint_name): |
|
|
50 |
if '_Nub' in joint_name: |
|
|
51 |
anybody_position = self.joint_values[joint_name]['r'] |
|
|
52 |
else: |
|
|
53 |
anybody_position = self.joint_values[joint_name]['r0'] |
|
|
54 |
return np.matmul(ROTATION_KS, np.array(anybody_position) * 1000) |