joint_angle_error.py 6.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141
  1. import csv
  2. import os
  3. import numpy as np
  4. sum_shoulder_left_error = 0
  5. sum_elbow_left_error = 0
  6. sum_shoulder_right_error = 0
  7. sum_elbow_right_error = 0
  8. sum_knee_left_error = 0
  9. sum_ankle_left_error = 0
  10. sum_knee_right_error = 0
  11. sum_ankle_right_error = 0
  12. def get_vector(row, joint_name):
  13. """
  14. Get vector from csv row and joint_name (convert string to float)
  15. Parameters:
  16. row: row from csv
  17. joint_name: name of joint
  18. Returns:
  19. vector: vector with type float
  20. """
  21. vector = row[joint_name].split(';')
  22. vector = list(map(float, vector))
  23. return vector
  24. def get_angle(a, b, c):
  25. """
  26. Get angle b (imagine there are line ba and bc)
  27. Parameters:
  28. a: Vector a
  29. b: Vector b
  30. c: Vector c
  31. Returns:
  32. angle: Angle b in degrees
  33. """
  34. p = np.subtract(a, b)
  35. q = np.subtract(c, b)
  36. angle = np.arccos(np.dot(p, q) / (np.linalg.norm(p) * np.linalg.norm(q)))
  37. return np.rad2deg(angle)
  38. path = os.path.join(os.getcwd(), 'Assets\\demo_and_body_positions.csv')
  39. with open(path, newline='') as csvfile:
  40. reader = csv.DictReader(csvfile)
  41. for row in reader:
  42. demo_spine_shoulder = get_vector(row, 'Demo_SpineShoulder')
  43. demo_shoulder_left = get_vector(row, 'Demo_ShoulderLeft')
  44. demo_elbow_left = get_vector(row, 'Demo_ElbowLeft')
  45. demo_wrist_left = get_vector(row, 'Demo_WristLeft')
  46. demo_shoulder_right = get_vector(row, 'Demo_ShoulderRight')
  47. demo_elbow_right = get_vector(row, 'Demo_ElbowRight')
  48. demo_wrist_right = get_vector(row, 'Demo_WristRight')
  49. demo_hip_left = get_vector(row, 'Demo_HipLeft')
  50. demo_knee_left = get_vector(row, 'Demo_KneeLeft')
  51. demo_ankle_left = get_vector(row, 'Demo_AnkleLeft')
  52. demo_foot_left = get_vector(row, 'Demo_FootLeft')
  53. demo_hip_right = get_vector(row, 'Demo_HipRight')
  54. demo_knee_right = get_vector(row, 'Demo_KneeRight')
  55. demo_ankle_right = get_vector(row, 'Demo_AnkleRight')
  56. demo_foot_right = get_vector(row, 'Demo_FootRight')
  57. body_spine_shoulder = get_vector(row, 'Body_SpineShoulder')
  58. body_shoulder_left = get_vector(row, 'Body_ShoulderLeft')
  59. body_elbow_left = get_vector(row, 'Body_ElbowLeft')
  60. body_wrist_left = get_vector(row, 'Body_WristLeft')
  61. body_shoulder_right = get_vector(row, 'Body_ShoulderRight')
  62. body_elbow_right = get_vector(row, 'Body_ElbowRight')
  63. body_wrist_right = get_vector(row, 'Body_WristRight')
  64. body_hip_left = get_vector(row, 'Body_HipLeft')
  65. body_knee_left = get_vector(row, 'Body_KneeLeft')
  66. body_ankle_left = get_vector(row, 'Body_AnkleLeft')
  67. body_foot_left = get_vector(row, 'Body_FootLeft')
  68. body_hip_right = get_vector(row, 'Body_HipRight')
  69. body_knee_right = get_vector(row, 'Body_KneeRight')
  70. body_ankle_right = get_vector(row, 'Body_AnkleRight')
  71. body_foot_right = get_vector(row, 'Body_FootRight')
  72. # Shoulder left
  73. demo_angle_shoulder_left = get_angle(demo_spine_shoulder, demo_shoulder_left, demo_elbow_left)
  74. body_angle_shoulder_left = get_angle(body_spine_shoulder, body_shoulder_left, body_elbow_left)
  75. shoulder_left_error = abs(demo_angle_shoulder_left - body_angle_shoulder_left)
  76. sum_shoulder_left_error += shoulder_left_error
  77. # Elbow left
  78. demo_angle_elbow_left = get_angle(demo_shoulder_left, demo_elbow_left, demo_wrist_left)
  79. body_angle_elbow_left = get_angle(body_shoulder_left, body_elbow_left, body_wrist_left)
  80. elbow_left_error = abs(demo_angle_elbow_left - body_angle_elbow_left)
  81. sum_elbow_left_error += elbow_left_error
  82. # Shoulder right
  83. demo_angle_shoulder_right = get_angle(demo_spine_shoulder, demo_shoulder_right, demo_elbow_right)
  84. body_angle_shoulder_right = get_angle(body_spine_shoulder, body_shoulder_right, body_elbow_right)
  85. shoulder_right_error = abs(demo_angle_shoulder_right - body_angle_shoulder_right)
  86. sum_shoulder_right_error += shoulder_right_error
  87. # Elbow right
  88. demo_angle_elbow_right = get_angle(demo_shoulder_right, demo_elbow_right, demo_wrist_right)
  89. body_angle_elbow_right = get_angle(body_shoulder_right, body_elbow_right, body_wrist_right)
  90. elbow_right_error = abs(demo_angle_elbow_right - body_angle_elbow_right)
  91. sum_elbow_right_error += elbow_right_error
  92. # Knee left
  93. demo_angle_knee_left = get_angle(demo_hip_left, demo_knee_left, demo_ankle_left)
  94. body_angle_knee_left = get_angle(body_hip_left, body_knee_left, body_ankle_left)
  95. knee_left_error = abs(demo_angle_knee_left - body_angle_knee_left)
  96. sum_knee_left_error += knee_left_error
  97. # Ankle left
  98. demo_angle_ankle_left = get_angle(demo_knee_left, demo_ankle_left, demo_foot_left)
  99. body_angle_ankle_left = get_angle(body_knee_left, body_ankle_left, body_foot_left)
  100. ankle_left_error = abs(demo_angle_ankle_left - body_angle_ankle_left)
  101. sum_ankle_left_error += ankle_left_error
  102. # Knee right
  103. demo_angle_knee_right = get_angle(demo_hip_right, demo_knee_right, demo_ankle_right)
  104. body_angle_knee_right = get_angle(body_hip_right, body_knee_right, body_ankle_right)
  105. knee_right_error = abs(demo_angle_knee_right - body_angle_knee_right)
  106. sum_knee_right_error += knee_right_error
  107. # Ankle right
  108. demo_angle_ankle_right = get_angle(demo_knee_right, demo_ankle_right, demo_foot_right)
  109. body_angle_ankle_right = get_angle(body_knee_right, body_ankle_right, body_foot_right)
  110. ankle_right_error = abs(demo_angle_ankle_right - body_angle_ankle_right)
  111. sum_ankle_right_error += ankle_right_error
  112. size = reader.line_num - 1
  113. print("shoulder_left_error: ", sum_shoulder_left_error / size)
  114. print("elbow_left_error: ", sum_elbow_left_error / size)
  115. print("shoulder_right_error: ", sum_shoulder_right_error / size)
  116. print("elbow_right_error: ", sum_elbow_right_error / size)
  117. print("knee_left_error: ", sum_knee_left_error / size)
  118. print("ankle_left_error: ", sum_ankle_left_error / size)
  119. print("knee_right_error: ", sum_knee_right_error / size)
  120. print("ankle_right_error: ", sum_ankle_right_error / size)