├── ergonomics
    ├── __init__.py
    ├── utils.py
    ├── owas.py
    └── reba.py
├── Resources
    └── pose.gif
├── setup.py
├── LICENSE
└── README.md


/ergonomics/__init__.py:
--------------------------------------------------------------------------------
1 | 


--------------------------------------------------------------------------------
/Resources/pose.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/rs9000/ergonomics/HEAD/Resources/pose.gif


--------------------------------------------------------------------------------
/setup.py:
--------------------------------------------------------------------------------
 1 | import setuptools
 2 | 
 3 | with open("README.md", "r") as fh:
 4 |     long_description = fh.read()
 5 | 
 6 | setuptools.setup(
 7 |      name='ergonomics_metrics',
 8 |      version='0.2',
 9 |      author="Rosario Di Carlo",
10 |      author_email="rs.dicarlo@gmail.com",
11 |      description="Ergonomics metrics",
12 |      long_description="Compute ergonomics metrics from human pose",
13 |      long_description_content_type="text/markdown",
14 |      url="https://github.com/rs9000/ergonomics",
15 |      packages=setuptools.find_packages(),
16 |      classifiers=[
17 |          "Programming Language :: Python :: 3",
18 |          "License :: OSI Approved :: MIT License",
19 |          "Operating System :: OS Independent",
20 |      ],
21 |  )


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2019 Rs
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Ergonomics
 2 | Ergonomics metrics: REBA score
 3 | 
 4 | <img src="./Resources/pose.gif" width="600"></th>
 5 | 
 6 | 
 7 | ### Installation
 8 | 
 9 | ```
10 | pip install ergonomics-metrics
11 | ```
12 | 
13 | ### Example
14 | 
15 | ```python
16 | from ergonomics.reba import RebaScore
17 | import requests
18 | import numpy as np
19 | import json
20 | 
21 | url = 'https://www.dropbox.com/s/e72lwn9n9gkoygj/ergonomics.json?dl=1'
22 | r = requests.get(url)
23 | sample_pose = np.array(r.json()['sample_pose'])
24 | 
25 | rebaScore = RebaScore()
26 | 
27 | body_params = rebaScore.get_body_angles_from_pose_right(sample_pose)
28 | arms_params = rebaScore.get_arms_angles_from_pose_right(sample_pose)
29 | 
30 | rebaScore.set_body(body_params)
31 | score_a, partial_a = rebaScore.compute_score_a()
32 | 
33 | rebaScore.set_arms(arms_params)
34 | score_b, partial_b = rebaScore.compute_score_b()
35 | 
36 | score_c, caption = rebaScore.compute_score_c(score_a, score_b)
37 | 
38 | print("Score A: ", score_a, "Partial: ", partial_a)
39 | print("Score A: ", score_b, "Partial: ", partial_b)
40 | print("Score C: ", score_c, caption)
41 | 
42 | ```
43 | 
44 | 
45 | ### Input pose
46 | 
47 | The input pose is a 13+2 joints representing X,Y,Z coordinates relative to the root joint.
48 | ```
49 | [0] = Head
50 | [1] = Nose
51 | [2, 3, 4, 14]: Left Shoulder, Elbow, Wrist + Hand (optional)
52 | [5, 6, 7, 15]: Right Shoulder, Elbow, Wrist + Hand (optional)
53 | [8, 9, 10]: Left Hip, Knee, Ankle
54 | [11, 12, 13]: Right Hip, Knee, Ankle
55 | ```
56 | The root joint has coordinates (0,0,0) and can be arbitrarily chosen from the 15 joints in the pose. 
57 | During the calculation of the REBA score the root joint will be automatically set to the point of coordinates: (Lhip + Rhip)/2.
58 | 
59 | 
60 | ### Output
61 | 
62 | 
63 | ```
64 | Pose Left
65 | 
66 | Neck Score: 2
67 | Trunk Score: 2
68 | Legs Score: 2
69 | Upper Arm Score: 1
70 | Lower Arm Score: 2
71 | Wrist Score: 3
72 | 
73 | A Score: 4
74 | B Score: 3
75 | C score: 4
76 | REBA score: 4
77 | 
78 | Pose Right
79 | 
80 | Neck Score: 2
81 | Trunk Score: 2
82 | Legs Score: 2
83 | Upper Arm Score: 1
84 | Lower Arm Score: 2
85 | Wrist Score: 2
86 | 
87 | A Score: 4
88 | B Score: 2
89 | C score: 4
90 | REBA score: 4
91 | 
92 | Rank
93 | [0]'Negligible Risk',
94 | [2-3]'Low Risk. Change may be needed',
95 | [4-7]'Medium Risk. Further Investigate. Change Soon',
96 | [8-10]'High Risk. Investigate and Implement Change',
97 | [11+]'Very High Risk. Implement Change'
98 | ```
99 | 


--------------------------------------------------------------------------------
/ergonomics/utils.py:
--------------------------------------------------------------------------------
 1 | # -*- coding: utf-8 -*-
 2 | # ---------------------
 3 | 
 4 | import matplotlib
 5 | 
 6 | 
 7 | from matplotlib import figure
 8 | from matplotlib import cm
 9 | import numpy as np
10 | from typing import *
11 | from matplotlib import pyplot as plt
12 | from mpl_toolkits.mplot3d import Axes3D
13 | from mpl_toolkits.mplot3d.art3d import Poly3DCollection
14 | from random import randint
15 | 
16 | 
17 | def show_skeleton(joints, swap_y=False, title=None):
18 | 
19 |     if joints.shape[1] == 14:
20 |         bone_list = [[0, 1], [1, 2], [2, 3], [3, 4], [1, 5], [5, 6], [6, 7],
21 |                      [1, 8], [8, 9], [9, 10], [1, 11], [11, 12], [12, 13]]
22 | 
23 |     if joints.shape[1]  == 16:
24 |         bone_list = [[0, 1], [1, 2], [2, 3], [3, 4], [4, 14], [1, 5], [5, 6], [6, 7], [7, 15],
25 |                      [1, 8], [8, 9], [9, 10], [1, 11], [11, 12], [12, 13]]
26 | 
27 |     elif joints.shape[1]  == 24:
28 |         bone_list = [[0, 1], [1, 4], [4, 7], [7, 10], [0, 2], [2, 5], [5, 8],
29 |                      [8, 11], [0, 3], [3, 6], [6, 9], [9, 14], [14, 17], [17, 19],
30 |                      [19, 21], [21, 23], [9, 13], [13, 16], [16, 18], [18, 20], [20, 22],
31 |                      [16, 12], [17, 12], [12, 15]
32 |                      ]
33 | 
34 |     elif joints.shape[1]  == 32:
35 |         bone_list=[[0, 1], [1, 2], [2, 3], [3, 4], [4, 5],
36 |                     [0, 6], [6, 7], [7, 8], [8, 9], [9, 10],
37 |                     [0, 11], [11, 12], [12, 13], [13, 14],
38 |                     [14, 15],
39 |                     [16, 17], [17, 18], [18, 19], [19, 20],
40 |                     [20, 21], [21, 22],
41 |                     [22, 23],
42 |                     [16, 24], [24, 25], [25, 26], [26, 27],
43 |                     [27, 28], [28, 29], [29, 30], [30, 31]
44 |                     ]
45 | 
46 |     plt.figure()
47 |     ax = plt.axes(projection='3d')
48 |     if title is not None:
49 |         plt.title(title)
50 |     ax.set_xlabel("X")
51 |     ax.set_ylabel("Y")
52 |     ax.set_xlim(-0.7, 0.7)
53 |     ax.set_ylim(0.7, -0.7) if swap_y else ax.set_ylim(-0.7, 0.7)
54 |     ax.set_zlim(-0.7, 0.7)
55 |     ax.view_init(azim=-90, elev=100)
56 | 
57 |     for joint in joints:
58 |         color = (randint(64, 255) / 255, randint(64, 255) / 255, randint(64, 255) / 255)
59 |         x, y, z = joint.T[0], joint.T[1], joint.T[2]
60 | 
61 |         ax.scatter3D(x, y, z, color=color)
62 |         for bone in bone_list:
63 |             ax.plot3D([x[bone[0]], x[bone[1]]], [y[bone[0]], y[bone[1]]], [z[bone[0]], z[bone[1]]], color=color)
64 | 
65 |     plt.show()
66 | 
67 | 
68 | def rotate_pose(data, rotation_joint=8, rotation_matrix=None, m_coeff=None):
69 | 
70 |     if rotation_matrix is None:
71 |         rotation_matrix = np.zeros((data.shape[0], 3, 3))
72 | 
73 |         for i in range(data.shape[0]):
74 | 
75 |             # X, Z coordinates of ankle joint
76 |             m = np.arctan2(data[i, rotation_joint, 0], data[i, rotation_joint, 2])
77 | 
78 |             if m_coeff is not None:
79 |                 m=m_coeff
80 | 
81 |             # Rotation Matrix
82 |             R = np.array(([np.cos(m), 0, np.sin(m)],
83 |                           [0, 1, 0],
84 |                           [-np.sin(m), 0, np.cos(m)]))
85 | 
86 |             rotation_matrix[i] = R
87 | 
88 |     data = np.matmul(data, rotation_matrix)
89 |     return data, rotation_matrix
90 | 


--------------------------------------------------------------------------------
/ergonomics/owas.py:
--------------------------------------------------------------------------------
  1 | # -*- coding: utf-8 -*-
  2 | # ---------------------
  3 | 
  4 | import numpy as np
  5 | import ergonomics.utils as utils
  6 | 
  7 | class OwasScore:
  8 |     '''
  9 |     Class to compute OWAS metrics
 10 | 
 11 |     Pose:
 12 |           [0]: Head
 13 |           [1]: Neck
 14 |           [2, 3, 4, 14]: Left arm + (optional)left hand
 15 |           [5, 6, 7, 15]: Right arm + (optional)right hand
 16 |           [8, 9, 10]: Left leg
 17 |           [11, 12, 13]: Right leg
 18 |     '''
 19 | 
 20 |     def __init__(self):
 21 | 
 22 |         # Body Params
 23 |         self.body_params = {'trunk_angle': 0, 'trunk_side_angle': 0, 'arm_step_left': 0, 'arm_step_right': 0,
 24 |                             'legs_angle_left': 0, 'legs_angle_right': 0, 'knee_angle_left': 0, 'knee_angle_right': 0,
 25 |                             'knee_offset_left': 0, 'knee_offset_right': 0, 'step_size_y': 0, 'step_size_x': 0
 26 |                             }
 27 | 
 28 |         # Init lookup tables
 29 |         self.table = np.ones((4, 3, 7, 3)).astype(int)
 30 |         self.init_table()
 31 | 
 32 |     def init_table(self):
 33 |         '''
 34 |         Table used to compute OWAS score
 35 | 
 36 |         :return: None
 37 |         '''
 38 | 
 39 |         self.table[0] = [
 40 |                             [[1, 1, 1], [1, 1, 1], [1, 1, 1], [2, 2, 2], [2, 2, 2], [1, 1, 1], [1, 1, 1]],
 41 |                             [[1, 1, 1], [1, 1, 1], [1, 1, 1], [2, 2, 2], [2, 2, 2], [1, 1, 1], [1, 1, 1]],
 42 |                             [[1, 1, 1], [1, 1, 1], [1, 1, 1], [2, 2, 3], [2, 2, 3], [1, 1, 1], [1, 1, 2]]
 43 |                         ]
 44 | 
 45 |         self.table[1] = [
 46 |                             [[2, 2, 3], [2, 2, 3], [2, 2, 3], [3, 3, 3], [3, 3, 3], [2, 2, 2], [2, 3, 3]],
 47 |                             [[2, 2, 3], [2, 2, 3], [2, 3, 3], [3, 4, 4], [3, 4, 4], [3, 3, 4], [2, 3, 4]],
 48 |                             [[3, 3, 4], [2, 2, 3], [3, 3, 3], [3, 4, 4], [4, 4, 4], [4, 4, 4], [2, 3, 4]]
 49 |                         ]
 50 | 
 51 |         self.table[2] = [
 52 |                             [[1, 1, 1], [1, 1, 1], [1, 1, 2], [3, 3, 3], [4, 4, 4], [1, 1, 1], [1, 1, 1]],
 53 |                             [[2, 2, 3], [1, 1, 1], [1, 1, 2], [4, 4, 4], [4, 4, 4], [3, 3, 3], [1, 1, 1]],
 54 |                             [[2, 2, 3], [1, 1, 1], [2, 3, 3], [4, 4, 4], [4, 4, 4], [4, 4, 4], [1, 1, 2]]
 55 |                         ]
 56 | 
 57 |         self.table[3] = [
 58 |                             [[2, 3, 3], [2, 2, 3], [2, 3, 3], [4, 4, 4], [4, 4, 4], [4, 4, 4], [2, 3, 4]],
 59 |                             [[3, 3, 4], [2, 3, 4], [3, 3, 4], [4, 4, 4], [4, 4, 4], [4, 4, 4], [2, 3, 4]],
 60 |                             [[4, 4, 4], [2, 3, 4], [3, 3, 4], [4, 4, 4], [4, 4, 4], [4, 4, 4], [2, 3, 4]]
 61 |                         ]
 62 | 
 63 |     def set_body_params(self, values):
 64 |         # type: (np.ndarray) -> None
 65 |         '''
 66 |         Set body params
 67 | 
 68 |         :param values: [trunk_angle, trunk_side_angle, arm_step_left, arm_step_right, legs_angle_left,
 69 |                         legs_angle_right, knee_angle_left, knee_angle_right, knee_offset_left,
 70 |                         knee_offset_right, step_size_y, step_size_x]
 71 |         :return: None
 72 |         '''
 73 |         assert len(values) == len(self.body_params)
 74 | 
 75 |         for i, (key, _) in enumerate(self.body_params.items()):
 76 |             self.body_params[key] = values[i]
 77 | 
 78 |     def compute_score(self):
 79 |         # type: (OwasScore) -> (np.ndarray, np.ndarray)
 80 |         '''
 81 |         Compute OWAS score
 82 |         >>> owascore = OwasScore()
 83 |         >>> owascore.set_body_params(np.array([10, 1, -0.1, 0.1, 10, 10, 160, 160, 0.4, 0.4, 0.1, 0.1]))
 84 |         >>> owascore.compute_score()
 85 |         (1, array([1, 2, 2, 1]))
 86 | 
 87 |         :return: OWAS_score, [neck_score, trunk_score, leg_score]
 88 |         '''
 89 | 
 90 |         trunk, arms, legs, load = 0, 1, 0, 0
 91 |         trunk = 1 if self.body_params['trunk_angle'] < 20 else 2
 92 | 
 93 |         if self.body_params['trunk_side_angle'] > 30:
 94 |             if trunk == 1:
 95 |                 trunk = 3
 96 |             elif trunk == 2:
 97 |                 trunk = 4
 98 | 
 99 |         if self.body_params['arm_step_left'] > 0:
100 |             arms += 1
101 | 
102 |         if self.body_params['arm_step_right'] > 0:
103 |             arms += 1
104 | 
105 |         # Is walking?
106 |         if self.body_params['step_size_x'] > 0.2:
107 |             legs = 7  # yes
108 |         else:
109 |             legs = 2  # no
110 | 
111 |         # On knee
112 |         if self.body_params['knee_offset_left'] < 0.1 or self.body_params['knee_offset_right'] < 0.1:
113 |             legs = 6
114 | 
115 |         # Sitting
116 |         if self.body_params['legs_angle_left'] > 80 and self.body_params['legs_angle_right'] > 80:
117 |             legs = 1
118 | 
119 |         # Both legs bent
120 |         if self.body_params['knee_angle_left'] < 95 and self.body_params['knee_angle_right'] < 95:
121 |             legs = 4
122 | 
123 |         # Weight on one leg + eventual bent
124 |         if self.body_params['step_size_y'] > 0.1:
125 |             legs = 5 if self.body_params['knee_angle_left'] < 95 or self.body_params['knee_angle_right'] < 95 else 3
126 | 
127 |         # Load
128 |         load = 1
129 | 
130 |         assert 0 < trunk < 4 and 0 < arms < 3 and 0 < legs < 7 and 0 < load < 3
131 |         owas_score = self.table[trunk-1][arms-1][legs-1][load-1]
132 |         return owas_score, np.array([trunk, arms, legs, load])
133 | 
134 |     @staticmethod
135 |     def get_param_from_pose(pose, verbose=False):
136 |         # type: (np.ndarray, bool) -> np.ndarray
137 |         '''
138 |         Get body params from pose
139 | 
140 |         :param pose: Pose (Joints coordinates)
141 |         :param verbose: If true show each pose for debugging
142 | 
143 |         :return: Body params [trunk_angle, trunk_side_angle, arm_step_left, arm_step_right, legs_angle_left,
144 |                               legs_angle_right, knee_angle_left, knee_angle_right, knee_offset_left,
145 |                               knee_offset_right, step_size_y, step_size_x]
146 |         '''
147 |         pose = np.expand_dims(np.copy(pose), 0)
148 | 
149 |         if verbose:
150 |             utils.show_skeleton(pose, title="GT pose")
151 | 
152 |         # Trunk position
153 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8)
154 |         pose -= (pose[:, 8] + pose[:, 11]) / 2
155 |         trunk_angle = np.rad2deg(np.arctan2(pose[0, 1, 1], pose[0, 1, 0]) - (np.pi / 2))
156 | 
157 |         if verbose:
158 |             utils.show_skeleton(pose, title="Trunk angle: " +str(round(trunk_angle, 2)))
159 | 
160 |         # Trunk twist
161 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi/2)
162 |         trunk_side_angle = abs(np.rad2deg(np.arctan2(pose[0, 1, 1], pose[0, 1, 0]) - (np.pi / 2)))
163 | 
164 |         if verbose:
165 |             utils.show_skeleton(pose, title="Trunk side angle: " + str(round(trunk_side_angle, 2)))
166 | 
167 |         # Arms position
168 |         arm_step_left =  pose[0, 3, 1] - pose[0, 2, 1]
169 |         arm_step_right = pose[0, 6, 1] - pose[0, 5, 1]
170 | 
171 |         # Legs position
172 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=-np.pi / 2)
173 |         pose -= pose[:, 8]
174 |         legs_angle_left = -np.rad2deg(np.arctan2(pose[0, 9, 1], pose[0, 9, 0]) + (np.pi / 2))
175 |         pose -= pose[:, 11]
176 |         legs_angle_right = -np.rad2deg(np.arctan2(pose[0, 12, 1], pose[0, 12, 0]) + (np.pi / 2))
177 | 
178 |         if verbose:
179 |             utils.show_skeleton(pose, title="Legs angle left: " + str(round(legs_angle_left, 2))
180 |                                              + " Legs angle right: " + str(round(legs_angle_right, 2)))
181 | 
182 |         pose -= pose[:, 9]
183 |         a = np.rad2deg(np.arctan2(pose[0, 10, 1], pose[0, 10, 0]))
184 |         b = np.rad2deg(np.arctan2(pose[0, 8, 1], pose[0, 8, 0]))
185 |         knee_angle_left = abs(a) + abs(b)
186 |         pose -= pose[:, 12]
187 |         a = np.rad2deg(np.arctan2(pose[0, 13, 1], pose[0, 13, 0]))
188 |         b = np.rad2deg(np.arctan2(pose[0, 11, 1], pose[0, 11, 0]))
189 |         knee_angle_right = abs(a) + abs(b)
190 | 
191 |         if verbose:
192 |             utils.show_skeleton(pose, title="Knee angle left: " + str(round(knee_angle_left, 2))
193 |                                              + " Knee angle right: " + str(round(knee_angle_right, 2)))
194 | 
195 |         knee_offset_left = abs(pose[0, 9, 1] - pose[0, 10, 1])
196 |         knee_offset_right = abs(pose[0, 12, 1] - pose[0, 13, 1])
197 | 
198 |         step_size_y = abs(pose[0, 10, 1] - pose[0, 13, 1])
199 |         step_size_x = abs(pose[0, 10, 0] - pose[0, 13, 0])
200 | 
201 |         return np.array([trunk_angle, trunk_side_angle, arm_step_left, arm_step_right, legs_angle_left,
202 |                          legs_angle_right, knee_angle_left, knee_angle_right, knee_offset_left,
203 |                          knee_offset_right, step_size_y, step_size_x])
204 | 
205 | 
206 | if __name__ == '__main__':
207 | 
208 |     import doctest
209 |     doctest.testmod()
210 | 
211 |     sample_pose = np.array([[ 0.08533354,  1.03611605,  0.09013124],
212 |                               [ 0.15391247,  0.91162637, -0.00353906],
213 |                               [ 0.22379057,  0.87361878,  0.11541229],
214 |                               [ 0.4084777 ,  0.69462843,  0.1775224 ],
215 |                               [ 0.31665226,  0.46389668,  0.16556387],
216 |                               [ 0.1239769 ,  0.82994377, -0.11715403],
217 |                               [ 0.08302169,  0.58146328, -0.19830338],
218 |                               [-0.06767788,  0.53928527, -0.00511249],
219 |                               [ 0.11368726,  0.49372503,  0.21275574],
220 |                               [ 0.069179  ,  0.07140968,  0.26841402],
221 |                               [ 0.10831762, -0.36339359,  0.34032449],
222 |                               [ 0.11368726,  0.41275504, -0.01171348],
223 |                               [ 0.        ,  0.        ,  0.        ],
224 |                               [ 0.02535541, -0.43954643,  0.04373671],
225 |                               [ 0.26709431,  0.33643749,  0.17985192],
226 |                               [-0.15117603,  0.49462711,  0.02703403]])
227 | 
228 |     owasScore = OwasScore()
229 | 
230 |     body_params = owasScore.get_param_from_pose(sample_pose, verbose=False)
231 |     owasScore.set_body_params(body_params)
232 |     owas_score, partial_score = owasScore.compute_score()
233 | 
234 |     print("Owas Score:", owas_score)
235 |     print("Trunk, Arms, Legs, Load :", partial_score)
236 | 
237 | 


--------------------------------------------------------------------------------
/ergonomics/reba.py:
--------------------------------------------------------------------------------
  1 | # -*- coding: utf-8 -*-
  2 | # ---------------------
  3 | 
  4 | import numpy as np
  5 | import ergonomics.utils as utils
  6 | 
  7 | 
  8 | class RebaScore:
  9 |     '''
 10 |     Class to compute REBA metrics
 11 | 
 12 |     Pose:
 13 |           [0]: Head
 14 |           [1]: Neck
 15 |           [2, 3, 4, 14]: Left arm + (optional)left hand
 16 |           [5, 6, 7, 15]: Right arm + (optional)right hand
 17 |           [8, 9, 10]: Left leg
 18 |           [11, 12, 13]: Right leg
 19 |     '''
 20 |     def __init__(self):
 21 |         # Table A ( Neck X Trunk X Legs)
 22 |         self.table_a = np.zeros((3, 5, 4))
 23 |         # Table B ( UpperArm X LowerArm X Wrist)
 24 |         self.table_b = np.zeros((6, 2, 3))
 25 |         # Table C ( ScoreA X ScoreB)
 26 |         self.table_c = np.zeros((12, 12))
 27 | 
 28 |         # Body Params
 29 |         self.body = {'neck_angle': 0, 'neck_side': False,
 30 |                      'trunk_angle': 0, 'trunk_side': False,
 31 |                      'legs_walking': False, 'legs_angle': 0,
 32 |                      'load': 0}
 33 | 
 34 |         # Arms Params
 35 |         self.arms = {'upper_arm_angle': 0, 'shoulder_raised': False, 'arm_abducted': False, 'leaning': False,
 36 |                      'lower_arm_angle': 0,
 37 |                      'wrist_angle': 0, 'wrist_twisted': False}
 38 | 
 39 |         # Init lookup tables
 40 |         self.init_table_a()
 41 |         self.init_table_b()
 42 |         self.init_table_c()
 43 | 
 44 | 
 45 |     def init_table_a(self):
 46 |         '''
 47 |         Table used to compute upper body score
 48 | 
 49 |         :return: None
 50 |         '''
 51 |         self.table_a = np.array([
 52 |                                 [[1, 2, 3, 4], [2, 3, 4, 5], [2, 4, 5, 6], [3, 5, 6, 7], [4, 6, 7, 8]],
 53 |                                 [[1, 2, 3, 4], [3, 4, 5, 6], [4, 5, 6, 7], [5, 6, 7, 8], [6, 7, 8, 9]],
 54 |                                 [[3, 3, 5, 6], [4, 5, 6, 7], [5, 6, 7, 8], [6, 7, 8, 9], [7, 8, 9, 9]]
 55 |                                 ])
 56 | 
 57 |     def init_table_b(self):
 58 |         '''
 59 |         Table used to computer lower body score
 60 | 
 61 |         :return: None
 62 |         '''
 63 |         self.table_b = np.array([
 64 |                                 [[1, 2, 2], [1, 2, 3]],
 65 |                                 [[1, 2, 3], [2, 3, 4]],
 66 |                                 [[3, 4, 5], [4, 5, 5]],
 67 |                                 [[4, 5, 5], [5, 6, 7]],
 68 |                                 [[6, 7, 8], [7, 8, 8]],
 69 |                                 [[7, 8, 8], [8, 9, 9]],
 70 |                                 ])
 71 | 
 72 |     def init_table_c(self):
 73 |         '''
 74 |         Table to compute score_c
 75 | 
 76 |         :return: None
 77 |         '''
 78 |         self.table_c = np.array([
 79 |                                 [1, 1, 1, 2, 3, 3, 4, 5, 6, 7, 7, 7],
 80 |                                 [1, 2, 2, 3, 4, 4, 5, 6, 6, 7, 7, 8],
 81 |                                 [2, 3, 3, 3, 4, 5, 6, 7, 7, 8, 8, 8],
 82 |                                 [3, 4, 4, 4, 5, 6, 7, 8, 8, 9, 9, 9],
 83 |                                 [4, 4, 4, 5, 6, 7, 8, 8, 9, 9, 9, 9],
 84 |                                 [6, 6, 6, 7, 8, 8, 9, 9, 10, 10, 10, 10],
 85 |                                 [7, 7, 7, 8, 9, 9, 9, 10, 10, 11, 11, 11],
 86 |                                 [8, 8, 8, 9, 10, 10, 10, 10, 10, 11, 11, 11],
 87 |                                 [9, 9, 9, 10, 10, 10, 11, 11, 11, 12, 12, 12],
 88 |                                 [10, 10, 10, 11, 11, 11, 11, 12, 12, 12, 12, 12],
 89 |                                 [11, 11, 11, 11, 12, 12, 12, 12, 12, 12, 12, 12],
 90 |                                 [12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12, 12],
 91 |                                 ])
 92 | 
 93 |     def set_body(self, values):
 94 |         # type: (np.ndarray) -> None
 95 |         '''
 96 |         Set body params
 97 | 
 98 |         :param values: [neck_angle, neck_side, trunk_angle, trunk_side,
 99 |                         legs_walking, legs_angle, load]
100 | 
101 |         :return: None
102 |         '''
103 |         assert len(values) == len(self.body)
104 | 
105 |         for i, (key, _) in enumerate(self.body.items()):
106 |             self.body[key] = values[i]
107 | 
108 |     def set_arms(self, values):
109 |         # type: (np.ndarray) -> None
110 |         '''
111 |         Set arms params
112 | 
113 |         :param values:  [upper_arm_angle, shoulder_raised, arm_abducted, leaning,
114 |                         lower_arm_angle, wrist_angle, wrist_twisted]
115 | 
116 |         :return: None
117 |         '''
118 |         assert len(values) == len(self.arms)
119 | 
120 |         for i, (key, _) in enumerate(self.arms.items()):
121 |             self.arms[key] = values[i]
122 | 
123 |     def compute_score_a(self):
124 |         # type: (RebaScore) -> (np.ndarray, np.ndarray)
125 |         '''
126 |         Compute score A
127 |         >>> rebascore = RebaScore()
128 |         >>> rebascore.set_body(np.array([10, 0, 20, 0, 1, 50, 0]))
129 |         >>> rebascore.compute_score_a()
130 |         (4, array([1, 2, 3]))
131 | 
132 |         :return: Score A, [neck_score, trunk_score, leg_score]
133 |         '''
134 |         neck_score, trunk_score, leg_score, load_score = 0, 0, 0, 0
135 | 
136 |         # Neck position
137 |         if 10 <= self.body['neck_angle'] <= 20 :
138 |             neck_score +=1
139 |         else:
140 |             neck_score +=2
141 |         # Neck adjust
142 |         neck_score +=1 if self.body['neck_side'] else 0
143 | 
144 |         # Trunk position
145 |         if 0 <= self.body['trunk_angle'] <= 1:
146 |             trunk_score +=1
147 |         elif self.body['trunk_angle'] <= 20:
148 |             trunk_score +=2
149 |         elif 20 <= self.body['trunk_angle'] <= 60:
150 |             trunk_score +=3
151 |         elif self.body['trunk_angle'] > 60:
152 |             trunk_score +=4
153 |         # Trunk adjust
154 |         trunk_score += 1 if self.body['trunk_side'] else 0
155 | 
156 |         # Legs position
157 |         leg_score += 2 if self.body['legs_walking'] else 1
158 |         # Legs adjust
159 |         if 30 <= self.body['legs_angle'] <= 60:
160 |             leg_score += 1
161 |         elif self.body['legs_angle'] > 60:
162 |             leg_score += 2
163 | 
164 |         # Load
165 |         if 5 <= self.body['load'] <= 10:
166 |             load_score += 1
167 |         elif self.body['load'] > 10:
168 |             load_score += 2
169 | 
170 |         assert neck_score > 0 and trunk_score > 0 and leg_score > 0
171 | 
172 |         score_a = self.table_a[neck_score-1][trunk_score-1][leg_score-1]
173 |         return score_a, np.array([neck_score, trunk_score, leg_score])
174 | 
175 |     def compute_score_b(self):
176 |         # type: (RebaScore) -> (np.ndarray, np.ndarray)
177 |         '''
178 |         Compute score B
179 |         >>> rebascore = RebaScore()
180 |         >>> rebascore.set_arms(np.array([45, 0, 0, 0, 70, 0, 1]))
181 |         >>> rebascore.compute_score_b()
182 |         (2, array([2, 1, 2]))
183 | 
184 |         :return: scoreB, [upper_arm_score, lower_arm_score, wrist_score]
185 |         '''
186 |         upper_arm_score, lower_arm_score, wrist_score = 0, 0, 0
187 | 
188 |         # Upper arm position
189 |         if -20 <= self.arms['upper_arm_angle'] <= 20:
190 |             upper_arm_score +=1
191 |         elif self.arms['upper_arm_angle'] <= 45:
192 |             upper_arm_score +=2
193 |         elif 45 <= self.arms['upper_arm_angle'] <= 90:
194 |             upper_arm_score +=3
195 |         elif self.arms['upper_arm_angle'] > 90:
196 |             upper_arm_score +=4
197 | 
198 |         # Upper arm adjust
199 |         upper_arm_score += 1 if self.arms['shoulder_raised'] else 0
200 |         upper_arm_score += 1 if self.arms['arm_abducted'] else 0
201 |         upper_arm_score -= 1 if self.arms['leaning'] else 0
202 | 
203 |         # Lower arm position
204 |         if 60 <= self.arms['lower_arm_angle'] <= 100:
205 |             lower_arm_score += 1
206 |         else:
207 |             lower_arm_score += 2
208 | 
209 |         # Wrist position
210 |         if -15 <= self.arms['wrist_angle'] <= 15:
211 |             wrist_score += 1
212 |         else:
213 |             wrist_score += 2
214 | 
215 |         # Wrist adjust
216 |         wrist_score += 1 if self.arms['wrist_twisted'] else 0
217 | 
218 |         assert lower_arm_score > 0 and wrist_score > 0
219 | 
220 |         score_b = self.table_b[upper_arm_score-1][lower_arm_score-1][wrist_score-1]
221 |         return score_b, np.array([upper_arm_score, lower_arm_score, wrist_score])
222 | 
223 |     def compute_score_c(self, score_a, score_b):
224 |         # type: (np.ndarray, np.ndarray) -> (np.ndarray, str)
225 |         '''
226 |         Compute score C
227 | 
228 |         :param score_a:  Score A
229 |         :param score_b:  Score B
230 | 
231 |         :return: Score C, caption
232 |         '''
233 |         reba_scoring = ['Negligible Risk',
234 |                          'Low Risk. Change may be needed',
235 |                          'Medium Risk. Further Investigate. Change Soon',
236 |                          'High Risk. Investigate and Implement Change',
237 |                          'Very High Risk. Implement Change'
238 |                          ]
239 | 
240 |         score_c = self.table_c[score_a-1][score_b-1]
241 |         ix = self.score_c_to_5_classes(score_c)
242 |         caption = reba_scoring[ix]
243 | 
244 |         return score_c, caption
245 | 
246 |     @staticmethod
247 |     def score_c_to_5_classes(score_c):
248 |         # type: (np.ndarray) -> int
249 |         '''
250 |         Score C to 5 risk-classes
251 | 
252 |         :param score_c:  Score C
253 |         :return: Risk-class
254 |         '''
255 |         if score_c == 1:
256 |             ret = 0
257 |         elif 2 <= score_c <= 3:
258 |             ret = 1
259 |         elif 4 <= score_c <= 7:
260 |             ret = 2
261 |         elif 8 <= score_c <= 10:
262 |             ret = 3
263 |         else:
264 |             ret = 4
265 | 
266 |         return ret
267 | 
268 |     @staticmethod
269 |     def get_body_angles_from_pose_left(pose, verbose=False):
270 |         # type: (np.ndarray, bool) -> np.ndarray
271 |         '''
272 |         Get body angles from pose (look at left)
273 | 
274 |         :param pose: Pose (Joints coordinates)
275 |         :param verbose: If true show each pose for debugging
276 | 
277 |         :return: Body params (neck_angle, neck_side, trunk_angle, trunk_side,
278 |                 legs_walking, legs_angle, load)
279 |         '''
280 | 
281 |         pose = np.expand_dims(np.copy(pose), 0)
282 | 
283 |         neck_angle, neck_side, trunk_angle, trunk_side, \
284 |         legs_walking, legs_angle, load = 0, 0, 0, 0, 0, 0, 0
285 | 
286 |         if verbose:
287 |             utils.show_skeleton(pose, title="GT pose")
288 |             _pose, _ = utils.rotate_pose(np.copy(pose), rotation_joint=8)
289 |             utils.show_skeleton(_pose, title="GT pose left")
290 | 
291 |         # Trunk position
292 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8)
293 |         pose -= (pose[:, 8] + pose[:, 11]) /2
294 | 
295 |         if quad(pose[0, 1]) < 3:
296 |             trunk_angle = np.rad2deg(np.arctan2(pose[0, 1, 1], pose[0, 1, 0]) - (np.pi / 2))
297 |         else:
298 |             trunk_angle = 270 + np.rad2deg(np.arctan2(pose[0, 1, 1], pose[0, 1, 0]))
299 | 
300 |         if verbose:
301 |             utils.show_skeleton(pose, title="Trunk angle: " + str(round(trunk_angle, 2)))
302 | 
303 |         # Trunk bending
304 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi/2)
305 | 
306 |         angle = np.pi /2 if quad(pose[0, 1]) > 2 else -np.pi/2
307 |         trunk_side_angle = abs(np.rad2deg(np.arctan2(pose[0, 1, 1], pose[0, 1, 0]) + angle))
308 |         trunk_side = 1 if trunk_side_angle > 30 else 0
309 | 
310 |         if verbose:
311 |             utils.show_skeleton(pose, title="Trunk side angle: " + str(round(trunk_side_angle, 2)))
312 | 
313 |         # Neck position
314 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=-np.pi/2)
315 |         pose -= pose[:, 1]
316 | 
317 |         if quad(pose[0, 0]) < 3:
318 |             neck_angle = np.rad2deg(np.arctan2(pose[0, 0, 1], pose[0, 0, 0]) - (np.pi / 2)) - trunk_angle
319 |         else:
320 |             neck_angle = 270 + np.rad2deg(np.arctan2(pose[0, 0, 1], pose[0, 0, 0])) - trunk_angle
321 | 
322 |         if verbose:
323 |             utils.show_skeleton(pose, title="Neck angle: " + str(round(neck_angle, 2)))
324 | 
325 |         # Neck bending
326 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi / 2)
327 |         angle = np.pi /2 if quad(pose[0, 0]) > 2 else -np.pi/2
328 |         neck_side_angle = abs(np.rad2deg(np.arctan2(pose[0, 0, 1], pose[0, 0, 0]) + angle)) - trunk_side_angle
329 |         neck_side = 1 if neck_side_angle > 20 else 0
330 | 
331 |         if verbose:
332 |             utils.show_skeleton(pose, title="Neck side angle: " + str(round(neck_side_angle, 2)))
333 | 
334 |         # Legs position
335 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=-np.pi / 2)
336 |         pose -= pose[:, 8]
337 | 
338 |         if quad(pose[0, 9]) > 2:
339 |             legs_angle = -np.rad2deg(np.arctan2(pose[0, 9, 1], pose[0, 9, 0]) + (np.pi/2))
340 |         else:
341 |             legs_angle = 270 - np.rad2deg(np.arctan2(pose[0, 9, 1], pose[0, 9, 0]))
342 | 
343 |         step_size = abs(np.linalg.norm(pose[0, 10, :2] - pose[0, 13, :2]))
344 |         legs_walking = 1 if step_size > 0.1 else 0
345 | 
346 |         if verbose:
347 |             title = "Leg angle: " + str(round(legs_angle, 2)) + " Step size: " + str(round(step_size, 2))
348 |             utils.show_skeleton(pose, title=title)
349 | 
350 |         return np.array([neck_angle, neck_side, trunk_angle, trunk_side,
351 |                            legs_walking, legs_angle, load])
352 | 
353 | 
354 |     @staticmethod
355 |     def get_body_angles_from_pose_right(pose, verbose=False):
356 |         # type: (np.ndarray, bool) -> np.ndarray
357 |         '''
358 |         Get body angles from pose (look at right)
359 | 
360 |         :param pose: Pose (Joints coordinates)
361 |         :param verbose: If true show each pose for debugging
362 | 
363 |         :return: Body params (neck_angle, neck_side, trunk_angle, trunk_side,
364 |                               legs_walking, legs_angle, load)
365 |         '''
366 |         pose = np.expand_dims(np.copy(pose), 0)
367 | 
368 |         neck_angle, neck_side, trunk_angle, trunk_side, \
369 |         legs_walking, legs_angle, load = 0, 0, 0, 0, 0, 0, 0
370 | 
371 |         if verbose:
372 |             utils.show_skeleton(pose, title="GT pose")
373 |             _pose, _ = utils.rotate_pose(np.copy(pose), rotation_joint=8)
374 |             _pose, _ = utils.rotate_pose(_pose, rotation_joint=8, m_coeff=np.pi)
375 |             utils.show_skeleton(_pose, title="GT pose Right")
376 | 
377 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8)
378 | 
379 |         # Trunk position
380 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi)
381 |         pose -= (pose[:, 8] + pose[:, 11]) / 2
382 |         trunk_angle = np.rad2deg((np.pi / 2)  - np.arctan2(pose[0, 1, 1], pose[0, 1, 0]))
383 | 
384 |         if verbose:
385 |             utils.show_skeleton(pose, title="Trunk angle: " + str(round(trunk_angle, 2)))
386 | 
387 |         # Trunk bending
388 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi / 2)
389 |         trunk_side_angle = abs(np.rad2deg(np.arctan2(pose[0, 1, 1], pose[0, 1, 0]) - (np.pi / 2)))
390 |         trunk_side = 1 if trunk_side_angle > 30 else 0
391 | 
392 |         if verbose:
393 |             utils.show_skeleton(pose, title="Trunk side angle: " + str(round(trunk_side_angle, 2)))
394 | 
395 |         # Neck position
396 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=-np.pi / 2)
397 |         pose -= pose[:, 1]
398 |         neck_angle = np.rad2deg((np.pi / 2) - np.arctan2(pose[0, 0, 1], pose[0, 0, 0])) - trunk_angle
399 | 
400 |         if verbose:
401 |             utils.show_skeleton(pose, title="Neck angle: " + str(round(neck_angle, 2)))
402 | 
403 |         # Neck bending
404 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi / 2)
405 |         neck_side_angle = np.abs(np.rad2deg(np.abs(np.arctan2(pose[0, 0, 1], pose[0, 0, 0])) - (np.pi / 2)))
406 |         neck_side = 1 if neck_side_angle > 20 else 0
407 | 
408 |         if verbose:
409 |             utils.show_skeleton(pose, title="Neck side angle: " + str(round(neck_side_angle, 2)))
410 | 
411 |         # Legs position
412 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=-np.pi / 2)
413 |         pose -= pose[:, 11]
414 |         legs_angle = np.rad2deg((np.pi / 2) + np.arctan2(pose[0, 12, 1], pose[0, 12, 0]))
415 |         step_size = abs(np.linalg.norm(pose[0, 10, :2] - pose[0, 13, :2]))
416 |         legs_walking = 1 if step_size > 0.1 else 0
417 | 
418 |         if verbose:
419 |             title = "Leg angle: " + str(round(legs_angle, 2)) + " Step size: " + str(round(step_size, 2))
420 |             utils.show_skeleton(pose, title=title)
421 | 
422 |         return np.array([neck_angle, neck_side, trunk_angle, trunk_side,
423 |                 legs_walking, legs_angle, load])
424 | 
425 |     @staticmethod
426 |     def get_arms_angles_from_pose_left(pose, verbose=False):
427 |         # type: (np.ndarray, bool) -> np.ndarray
428 |         '''
429 |         Get arms angles from pose (look at left)
430 | 
431 |         :param pose: Pose (Joints coordinates)
432 |         :param verbose: If true show each pose for debugging
433 | 
434 |         :return: Body params (upper_arm_angle, shoulder_raised, arm_abducted, leaning,
435 |                               lower_arm_angle, wrist_angle, wrist_twisted)
436 |         '''
437 |         pose = np.expand_dims(np.copy(pose), 0)
438 |         if verbose:
439 |             utils.show_skeleton(pose, title="GT pose")
440 | 
441 |         # Leaning
442 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8)
443 |         pose -= (pose[:, 8] + pose[:, 11]) / 2
444 | 
445 |         if quad(pose[0, 1]) < 3:
446 |             trunk_angle = np.rad2deg(np.arctan2(pose[0, 1, 1], pose[0, 1, 0]) - (np.pi / 2))
447 |         else:
448 |             trunk_angle = 270 + np.rad2deg(np.arctan2(pose[0, 1, 1], pose[0, 1, 0]))
449 | 
450 |         leaning = 1 if trunk_angle > 30 else 0
451 | 
452 |         if verbose:
453 |             utils.show_skeleton(pose, title="Leaning angle: " + str(round(trunk_angle, 2)))
454 | 
455 |         # Upper Arm position
456 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8)
457 |         pose -= pose[:, 2]
458 | 
459 |         if quad(pose[0, 3]) > 2:
460 |             upper_arm_angle = -np.rad2deg(np.arctan2(pose[0, 3, 1], pose[0, 3, 0]) + (np.pi / 2))
461 |         else:
462 |             upper_arm_angle = 270 - np.rad2deg(np.arctan2(pose[0, 3, 1], pose[0, 3, 0]))
463 | 
464 | 
465 |         upper_arm_angle += trunk_angle
466 | 
467 |         if verbose:
468 |             utils.show_skeleton(pose, title="Upper Arms angle: " + str(round(upper_arm_angle, 2)))
469 | 
470 |         # Upper Arm Adjust
471 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi/2)
472 |         shoulder_step = pose[:, 2, 1] - pose[:, 1, 1]
473 | 
474 |         if quad(pose[0, 3]) > 2:
475 |             arm_abducted_angle = -np.rad2deg(np.arctan2(pose[0, 3, 1], pose[0, 3, 0]) + (np.pi / 2))
476 |         else:
477 |             arm_abducted_angle = 270 - np.rad2deg(np.arctan2(pose[0, 3, 1], pose[0, 3, 0]))
478 | 
479 |         shoulder_raised = 1 if shoulder_step > 0.02 else 0
480 |         arm_abducted = 1 if arm_abducted_angle > 45 else 0
481 | 
482 |         if verbose:
483 |             print(shoulder_raised)
484 |             utils.show_skeleton(pose, title="Upper Arms abducted: " + str(round(arm_abducted_angle, 2)))
485 | 
486 |         # Lower Arm position
487 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=-np.pi/2)
488 |         pose -= pose[:, 3]
489 | 
490 |         if quad(pose[0, 4]) > 2:
491 |             lower_arm_angle = -np.rad2deg(np.arctan2(pose[0, 4, 1], pose[0, 4, 0]) + (np.pi / 2))
492 |         else:
493 |             lower_arm_angle = 270 - np.rad2deg(np.arctan2(pose[0, 4, 1], pose[0, 4, 0]))
494 | 
495 |         lower_arm_angle = lower_arm_angle + trunk_angle - upper_arm_angle
496 | 
497 |         if verbose:
498 |             utils.show_skeleton(pose, title="Lower Arms angle: " + str(round(lower_arm_angle, 2)))
499 | 
500 |         # Wrist position
501 |         wrist_angle = 0
502 |         wrist_twisted = 0
503 | 
504 |         if pose.shape[1] > 14:
505 |             pose -= pose[:, 4]
506 |             wrist_angle = np.rad2deg(np.arctan2(pose[0, 14, 1], pose[0, 14, 0]) - (np.pi / 2) )
507 | 
508 |             if verbose:
509 |                 utils.show_skeleton(pose, title="Wrist Angle: " + str(round(wrist_angle, 2)))
510 | 
511 |             pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=-np.pi / 2)
512 |             wrist_twisted_angle = abs(np.rad2deg(np.arctan2(pose[0, 14, 1], pose[0, 14, 0]) - (np.pi / 2)))
513 |             wrist_twisted = 1 if wrist_twisted_angle > 30 else 0
514 | 
515 |         return np.array([upper_arm_angle, shoulder_raised, arm_abducted, leaning,
516 |                 lower_arm_angle, wrist_angle, wrist_twisted])
517 | 
518 | 
519 |     @staticmethod
520 |     def get_arms_angles_from_pose_right(pose, verbose=False):
521 |         # type: (np.ndarray, bool) -> np.ndarray
522 |         '''
523 |         Get arms angles from pose (look at right)
524 | 
525 |         :param pose: Pose (Joints coordinates)
526 |         :param verbose: If true show each pose for debugging
527 | 
528 |         :return: Body params (upper_arm_angle, shoulder_raised, arm_abducted, leaning,
529 |                                lower_arm_angle, wrist_angle, wrist_twisted)
530 |         '''
531 |         pose = np.expand_dims(np.copy(pose), 0)
532 |         if verbose:
533 |             utils.show_skeleton(pose, title="GT pose")
534 | 
535 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8)
536 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi)
537 | 
538 |         # Leaning
539 |         pose -= (pose[:, 8] + pose[:, 11]) / 2
540 |         trunk_angle = np.rad2deg((np.pi / 2) - np.arctan2(pose[0, 1, 1], pose[0, 1, 0]))
541 |         leaning = 1 if trunk_angle > 60 else 0
542 | 
543 |         if verbose:
544 |             utils.show_skeleton(pose, title="Leaning angle: " + str(round(trunk_angle, 2)))
545 | 
546 |         # Upper Arm position
547 |         pose -= pose[:, 5]
548 |         if quad(pose[0, 6]) == 2:
549 |             upper_arm_angle = -(270 - np.rad2deg(np.arctan2(pose[0, 6, 1], pose[0, 6, 0])) - trunk_angle)
550 |         else:
551 |             upper_arm_angle = np.rad2deg((np.pi /2) + np.arctan2(pose[0, 6, 1], pose[0, 6, 0])) + trunk_angle
552 | 
553 |         if verbose:
554 |             utils.show_skeleton(pose, title="Upper Arms angle: " + str(round(upper_arm_angle, 2)))
555 | 
556 |         # Upper Arm Adjust
557 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi / 2)
558 |         shoulder_step = pose[:, 5, 1] - pose[:, 1, 1]
559 |         arm_abducted_angle = abs(np.rad2deg((np.pi / 2) + np.arctan2(pose[0, 6, 1], pose[0, 6, 0])))
560 |         shoulder_raised = 1 if shoulder_step > 0.02 else 0
561 |         arm_abducted = 1 if arm_abducted_angle > 45 else 0
562 | 
563 |         if verbose:
564 |             print(shoulder_raised)
565 |             utils.show_skeleton(pose, title="Upper Arms abducted: " + str(round(arm_abducted_angle, 2)))
566 | 
567 |         # Lower Arm position
568 |         pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=-np.pi / 2)
569 |         pose -= pose[:, 6]
570 |         lower_arm_angle = np.rad2deg((np.pi / 2) + np.arctan2(pose[0, 7, 1], pose[0, 7, 0]) ) + trunk_angle - upper_arm_angle
571 | 
572 |         if verbose:
573 |             utils.show_skeleton(pose, title="Lower Arms angle: " + str(round(lower_arm_angle, 2)))
574 | 
575 |         # Wrist position
576 |         wrist_angle = 0
577 |         wrist_twisted = 0
578 | 
579 |         if pose.shape[1] > 14:
580 |             pose -= pose[:, 7]
581 |             wrist_angle = np.rad2deg((np.pi / 2) + np.arctan2(pose[0, 15, 1], pose[0, 15, 0]))
582 | 
583 |             if verbose:
584 |                 utils.show_skeleton(pose, title="Wrist Angle: " + str(round(wrist_angle, 2)))
585 | 
586 |             pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=-np.pi / 2)
587 |             wrist_twisted_angle = abs(np.rad2deg((np.pi / 2) + np.arctan2(pose[0, 15, 1], pose[0, 15, 0])))
588 |             wrist_twisted = 1 if wrist_twisted_angle > 30 else 0
589 | 
590 | 
591 |         return np.array([upper_arm_angle, shoulder_raised, arm_abducted, leaning,
592 |                          lower_arm_angle, wrist_angle, wrist_twisted])
593 | 
594 | def quad(coord):
595 |     q = 0
596 |     if coord[0] >= 0 and coord[1] >= 0:
597 |         q = 1
598 |     elif coord[0] <= 0 and coord[1] >= 0:
599 |         q = 2
600 |     elif coord[0] <= 0 and coord[1] <= 0:
601 |         q = 3
602 |     elif coord[0] >= 0 and coord[1] <= 0:
603 |         q = 4
604 |     return q
605 | 
606 | if __name__ == '__main__':
607 | 
608 |     import doctest
609 |     doctest.testmod()
610 | 
611 |     sample_pose = np.array([[ 0.08533354,  1.03611605,  0.09013124],
612 |                       [ 0.15391247,  0.91162637, -0.00353906],
613 |                       [ 0.22379057,  0.87361878,  0.11541229],
614 |                       [ 0.4084777 ,  0.69462843,  0.1775224 ],
615 |                       [ 0.31665226,  0.46389668,  0.16556387],
616 |                       [ 0.1239769 ,  0.82994377, -0.11715403],
617 |                       [ 0.08302169,  0.58146328, -0.19830338],
618 |                       [-0.06767788,  0.53928527, -0.00511249],
619 |                       [ 0.11368726,  0.49372503,  0.21275574],
620 |                       [ 0.069179  ,  0.07140968,  0.26841402],
621 |                       [ 0.10831762, -0.36339359,  0.34032449],
622 |                       [ 0.11368726,  0.41275504, -0.01171348],
623 |                       [ 0.        ,  0.        ,  0.        ],
624 |                       [ 0.02535541, -0.43954643,  0.04373671],
625 |                       [ 0.26709431,  0.33643749,  0.17985192],
626 |                       [-0.15117603,  0.49462711,  0.02703403]])
627 | 
628 |     rebaScore = RebaScore()
629 | 
630 |     body_params = rebaScore.get_body_angles_from_pose_right(sample_pose)
631 |     arms_params = rebaScore.get_arms_angles_from_pose_right(sample_pose)
632 | 
633 |     rebaScore.set_body(body_params)
634 |     score_a, partial_a = rebaScore.compute_score_a()
635 | 
636 |     rebaScore.set_arms(arms_params)
637 |     score_b, partial_b = rebaScore.compute_score_b()
638 | 
639 |     score_c, caption = rebaScore.compute_score_c(score_a, score_b)
640 | 
641 |     print("Score A: ", score_a, "Partial: ", partial_a)
642 |     print("Score A: ", score_b, "Partial: ", partial_b)
643 |     print("Score C: ", score_c, caption)
644 | 


--------------------------------------------------------------------------------