Browse Source

Create joint_angle_error.py

Kenkart 2 years ago
parent
commit
f0f357640c
1 changed files with 142 additions and 0 deletions
  1. 142 0
      joint_angle_error.py

+ 142 - 0
joint_angle_error.py

@@ -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)