|
@@ -0,0 +1,142 @@
|
|
|
+import csv
|
|
|
+import os
|
|
|
+import numpy as np
|
|
|
+
|
|
|
+sum_shoulder_left_error = 0
|
|
|
+sum_elbow_left_error = 0
|
|
|
+sum_shoulder_right_error = 0
|
|
|
+sum_elbow_right_error = 0
|
|
|
+sum_knee_left_error = 0
|
|
|
+sum_ankle_left_error = 0
|
|
|
+sum_knee_right_error = 0
|
|
|
+sum_ankle_right_error = 0
|
|
|
+
|
|
|
+
|
|
|
+def get_vector(row, joint_name):
|
|
|
+ """
|
|
|
+ Get vector from csv row and joint_name (convert string to float)
|
|
|
+
|
|
|
+ Parameters:
|
|
|
+ row: row from csv
|
|
|
+ joint_name: name of joint
|
|
|
+ Returns:
|
|
|
+ vector: vector with type float
|
|
|
+ """
|
|
|
+ vector = row[joint_name].split(';')
|
|
|
+ vector = list(map(float, vector))
|
|
|
+ return vector
|
|
|
+
|
|
|
+
|
|
|
+def get_angle(a, b, c):
|
|
|
+ """
|
|
|
+ Get angle b (imagine there are line ba and bc)
|
|
|
+
|
|
|
+ Parameters:
|
|
|
+ a: Vector a
|
|
|
+ b: Vector b
|
|
|
+ c: Vector c
|
|
|
+ Returns:
|
|
|
+ angle: Angle b in degrees
|
|
|
+ """
|
|
|
+ p = np.subtract(a, b)
|
|
|
+ q = np.subtract(c, b)
|
|
|
+ angle = np.arccos(np.dot(p, q) / (np.linalg.norm(p) * np.linalg.norm(q)))
|
|
|
+ return np.rad2deg(angle)
|
|
|
+
|
|
|
+path = os.path.join(os.getcwd(), 'Assets\\demo_and_body_positions.csv')
|
|
|
+print("path: ", path)
|
|
|
+with open(path, newline='') as csvfile:
|
|
|
+ reader = csv.DictReader(csvfile)
|
|
|
+ for row in reader:
|
|
|
+ demo_spine_shoulder = get_vector(row, 'Demo_SpineShoulder')
|
|
|
+ demo_shoulder_left = get_vector(row, 'Demo_ShoulderLeft')
|
|
|
+ demo_elbow_left = get_vector(row, 'Demo_ElbowLeft')
|
|
|
+ demo_wrist_left = get_vector(row, 'Demo_WristLeft')
|
|
|
+ demo_shoulder_right = get_vector(row, 'Demo_ShoulderRight')
|
|
|
+ demo_elbow_right = get_vector(row, 'Demo_ElbowRight')
|
|
|
+ demo_wrist_right = get_vector(row, 'Demo_WristRight')
|
|
|
+
|
|
|
+ demo_hip_left = get_vector(row, 'Demo_HipLeft')
|
|
|
+ demo_knee_left = get_vector(row, 'Demo_KneeLeft')
|
|
|
+ demo_ankle_left = get_vector(row, 'Demo_AnkleLeft')
|
|
|
+ demo_foot_left = get_vector(row, 'Demo_FootLeft')
|
|
|
+ demo_hip_right = get_vector(row, 'Demo_HipRight')
|
|
|
+ demo_knee_right = get_vector(row, 'Demo_KneeRight')
|
|
|
+ demo_ankle_right = get_vector(row, 'Demo_AnkleRight')
|
|
|
+ demo_foot_right = get_vector(row, 'Demo_FootRight')
|
|
|
+
|
|
|
+ body_spine_shoulder = get_vector(row, 'Body_SpineShoulder')
|
|
|
+ body_shoulder_left = get_vector(row, 'Body_ShoulderLeft')
|
|
|
+ body_elbow_left = get_vector(row, 'Body_ElbowLeft')
|
|
|
+ body_wrist_left = get_vector(row, 'Body_WristLeft')
|
|
|
+ body_shoulder_right = get_vector(row, 'Body_ShoulderRight')
|
|
|
+ body_elbow_right = get_vector(row, 'Body_ElbowRight')
|
|
|
+ body_wrist_right = get_vector(row, 'Body_WristRight')
|
|
|
+
|
|
|
+ body_hip_left = get_vector(row, 'Body_HipLeft')
|
|
|
+ body_knee_left = get_vector(row, 'Body_KneeLeft')
|
|
|
+ body_ankle_left = get_vector(row, 'Body_AnkleLeft')
|
|
|
+ body_foot_left = get_vector(row, 'Body_FootLeft')
|
|
|
+ body_hip_right = get_vector(row, 'Body_HipRight')
|
|
|
+ body_knee_right = get_vector(row, 'Body_KneeRight')
|
|
|
+ body_ankle_right = get_vector(row, 'Body_AnkleRight')
|
|
|
+ body_foot_right = get_vector(row, 'Body_FootRight')
|
|
|
+
|
|
|
+ # Shoulder left
|
|
|
+ demo_angle_shoulder_left = get_angle(demo_spine_shoulder, demo_shoulder_left, demo_elbow_left)
|
|
|
+ body_angle_shoulder_left = get_angle(body_spine_shoulder, body_shoulder_left, body_elbow_left)
|
|
|
+ shoulder_left_error = abs(demo_angle_shoulder_left - body_angle_shoulder_left)
|
|
|
+ sum_shoulder_left_error += shoulder_left_error
|
|
|
+
|
|
|
+ # Elbow left
|
|
|
+ demo_angle_elbow_left = get_angle(demo_shoulder_left, demo_elbow_left, demo_wrist_left)
|
|
|
+ body_angle_elbow_left = get_angle(body_shoulder_left, body_elbow_left, body_wrist_left)
|
|
|
+ elbow_left_error = abs(demo_angle_elbow_left - body_angle_elbow_left)
|
|
|
+ sum_elbow_left_error += elbow_left_error
|
|
|
+
|
|
|
+ # Shoulder right
|
|
|
+ demo_angle_shoulder_right = get_angle(demo_spine_shoulder, demo_shoulder_right, demo_elbow_right)
|
|
|
+ body_angle_shoulder_right = get_angle(body_spine_shoulder, body_shoulder_right, body_elbow_right)
|
|
|
+ shoulder_right_error = abs(demo_angle_shoulder_right - body_angle_shoulder_right)
|
|
|
+ sum_shoulder_right_error += shoulder_right_error
|
|
|
+
|
|
|
+ # Elbow right
|
|
|
+ demo_angle_elbow_right = get_angle(demo_shoulder_right, demo_elbow_right, demo_wrist_right)
|
|
|
+ body_angle_elbow_right = get_angle(body_shoulder_right, body_elbow_right, body_wrist_right)
|
|
|
+ elbow_right_error = abs(demo_angle_elbow_right - body_angle_elbow_right)
|
|
|
+ sum_elbow_right_error += elbow_right_error
|
|
|
+
|
|
|
+ # Knee left
|
|
|
+ demo_angle_knee_left = get_angle(demo_hip_left, demo_knee_left, demo_ankle_left)
|
|
|
+ body_angle_knee_left = get_angle(body_hip_left, body_knee_left, body_ankle_left)
|
|
|
+ knee_left_error = abs(demo_angle_knee_left - body_angle_knee_left)
|
|
|
+ sum_knee_left_error += knee_left_error
|
|
|
+
|
|
|
+ # Ankle left
|
|
|
+ demo_angle_ankle_left = get_angle(demo_knee_left, demo_ankle_left, demo_foot_left)
|
|
|
+ body_angle_ankle_left = get_angle(body_knee_left, body_ankle_left, body_foot_left)
|
|
|
+ ankle_left_error = abs(demo_angle_ankle_left - body_angle_ankle_left)
|
|
|
+ sum_ankle_left_error += ankle_left_error
|
|
|
+
|
|
|
+ # Knee right
|
|
|
+ demo_angle_knee_right = get_angle(demo_hip_right, demo_knee_right, demo_ankle_right)
|
|
|
+ body_angle_knee_right = get_angle(body_hip_right, body_knee_right, body_ankle_right)
|
|
|
+ knee_right_error = abs(demo_angle_knee_right - body_angle_knee_right)
|
|
|
+ sum_knee_right_error += knee_right_error
|
|
|
+
|
|
|
+ # Ankle right
|
|
|
+ demo_angle_ankle_right = get_angle(demo_knee_right, demo_ankle_right, demo_foot_right)
|
|
|
+ body_angle_ankle_right = get_angle(body_knee_right, body_ankle_right, body_foot_right)
|
|
|
+ ankle_right_error = abs(demo_angle_ankle_right - body_angle_ankle_right)
|
|
|
+ sum_ankle_right_error += ankle_right_error
|
|
|
+
|
|
|
+ size = reader.line_num - 1
|
|
|
+ print("shoulder_left_error: ", sum_shoulder_left_error / size)
|
|
|
+ print("elbow_left_error: ", sum_elbow_left_error / size)
|
|
|
+ print("shoulder_right_error: ", sum_shoulder_right_error / size)
|
|
|
+ print("elbow_right_error: ", sum_elbow_right_error / size)
|
|
|
+
|
|
|
+ print("knee_left_error: ", sum_knee_left_error / size)
|
|
|
+ print("ankle_left_error: ", sum_ankle_left_error / size)
|
|
|
+ print("knee_right_error: ", sum_knee_right_error / size)
|
|
|
+ print("ankle_right_error: ", sum_ankle_right_error / size)
|