def lower_arm_degree(self): m_body_number = bodyNum.BodyPartNumber() right_arm_joint_numbers = m_body_number.right_arm() left_arm_joint_numbers = m_body_number.left_arm() right_shoulder_elbow_vector = self.joints[right_arm_joint_numbers[1]] - self.joints[right_arm_joint_numbers[0]] left_shoulder_elbow_vector = self.joints[left_arm_joint_numbers[1]] - self.joints[left_arm_joint_numbers[0]] right_elbow_wrist_vector = self.joints[right_arm_joint_numbers[2]] - self.joints[right_arm_joint_numbers[1]] left_elbow_wrist_vector = self.joints[left_arm_joint_numbers[2]] - self.joints[left_arm_joint_numbers[1]] # right and left arm degree in saggital plane right_degree = math.degrees(math.acos(np.dot(right_shoulder_elbow_vector, right_elbow_wrist_vector)) / ( (math.sqrt(np.dot(right_shoulder_elbow_vector, right_shoulder_elbow_vector))) * ( math.sqrt(np.dot(right_elbow_wrist_vector, right_elbow_wrist_vector))))) left_degree = math.degrees(math.acos(np.dot(left_shoulder_elbow_vector, left_elbow_wrist_vector)) / ( (math.sqrt(np.dot(left_shoulder_elbow_vector, left_shoulder_elbow_vector))) * ( math.sqrt(np.dot(left_elbow_wrist_vector, left_elbow_wrist_vector))))) self.file.write("Lower right arm flexion \n") self.file.write(str(right_degree)) self.file.write("\n") self.file.write("Lower left arm flexion \n") self.file.write(str(left_degree)) self.file.write("\n")
def upper_arm_flex(self): m_body_number = bodyNum.BodyPartNumber() right_upper_arm_joint_numbers = m_body_number.right_arm() left_upper_arm_joint_numbers = m_body_number.left_arm() right_upper_arm_vector = self.joints[right_upper_arm_joint_numbers[ 2]] - self.joints[right_upper_arm_joint_numbers[1]] left_upper_arm_vector = self.joints[left_upper_arm_joint_numbers[ 2]] - self.joints[left_upper_arm_joint_numbers[1]] normal_trunk_plane = self.trunk_plane() flex_right_upper_arm = 90 - math.degrees( math.acos( np.dot(right_upper_arm_vector, normal_trunk_plane) / (math.sqrt(np.dot(normal_trunk_plane, normal_trunk_plane)) * math.sqrt( np.dot(right_upper_arm_vector, right_upper_arm_vector))))) flex_left_upper_arm = 90 - math.degrees( math.acos( np.dot(left_upper_arm_vector, normal_trunk_plane) / (math.sqrt(np.dot(normal_trunk_plane, normal_trunk_plane)) * math.sqrt(np.dot(left_upper_arm_vector, left_upper_arm_vector))))) self.file.write("Upper right arm flexion \n") self.file.write(str(flex_right_upper_arm)) self.file.write("\n") self.file.write("Upper left arm flexion \n") self.file.write(str(flex_left_upper_arm)) self.file.write("\n")
def neck_side_calculator(self): m_body_number = bodyNum.BodyPartNumber() neck_joint_numbers = m_body_number.neck() trunk_joint_numbers = m_body_number.trunk_upper_body() normal_plane = self.trunk_plane() neck_vector = self.joints[neck_joint_numbers[1]] - self.joints[ neck_joint_numbers[0]] project_neck_on_trunk_plane = neck_vector - np.dot( neck_vector, normal_plane) * normal_plane spine_vector = self.joints[trunk_joint_numbers[2]] - self.joints[ trunk_joint_numbers[0]] neck_side_bending = math.degrees( math.acos( np.dot(project_neck_on_trunk_plane, spine_vector) / (math.sqrt( np.dot(project_neck_on_trunk_plane, project_neck_on_trunk_plane)) * math.sqrt(np.dot(spine_vector, spine_vector))))) self.file.write("Neck side bending \n") self.file.write(str(neck_side_bending)) self.file.write("\n")
def shoulder_rise(self): m_body_number = bodyNum.BodyPartNumber() trunk_joint_numbers = m_body_number.trunk_upper_body() right_shoulder_joint_numbers = m_body_number.right_shoulder() left_shoulder_joint_numbers = m_body_number.left_shoulder() spine_vector = self.joints[trunk_joint_numbers[0]] - self.joints[ trunk_joint_numbers[2]] right_shoulder_vector = self.joints[right_shoulder_joint_numbers[ 1]] - self.joints[right_shoulder_joint_numbers[0]] left_shoulder_vector = self.joints[left_shoulder_joint_numbers[ 1]] - self.joints[left_shoulder_joint_numbers[0]] right_shoulder_rise_degree = math.degrees( math.acos( np.dot(spine_vector, right_shoulder_vector) / (math.sqrt(np.dot(right_shoulder_vector, right_shoulder_vector)) * math.sqrt(np.dot(spine_vector, spine_vector))))) left_shoulder_rise_degree = math.degrees( math.acos( np.dot(spine_vector, left_shoulder_vector) / (math.sqrt(np.dot(left_shoulder_vector, left_shoulder_vector)) * math.sqrt(np.dot(spine_vector, spine_vector))))) self.file.write("Upper right arm raise \n") self.file.write(str(right_shoulder_rise_degree)) self.file.write("\n") self.file.write("Upper left arm raise \n") self.file.write(str(left_shoulder_rise_degree)) self.file.write("\n")
def wrist_side(self): m_body = bodyNum.BodyPartNumber() right_arm_joint_number = m_body.right_arm() left_arm_joint_number = m_body.left_arm() right_shoulder_elbow_vector = self.joints[ right_arm_joint_number[1]] - self.joints[right_arm_joint_number[0]] left_shoulder_elbow_vector = self.joints[ left_arm_joint_number[1]] - self.joints[left_arm_joint_number[0]] right_elbow_wrist_vector = self.joints[ right_arm_joint_number[2]] - self.joints[right_arm_joint_number[1]] left_elbow_wrist_vector = self.joints[ left_arm_joint_number[2]] - self.joints[left_arm_joint_number[1]] right_wrist_finger_vector = self.joints[ right_arm_joint_number[3]] - self.joints[right_arm_joint_number[2]] left_wrist_finger_vector = self.joints[ left_arm_joint_number[3]] - self.joints[left_arm_joint_number[2]] right_plane_normal_vec = np.cross(right_shoulder_elbow_vector, right_elbow_wrist_vector) left_plane_normal_vec = np.cross(left_shoulder_elbow_vector, left_elbow_wrist_vector) if right_plane_normal_vec[0] != 0 or right_plane_normal_vec[ 1] != 0 or right_plane_normal_vec[2] != 0: right_side_bent_degree = 90 - math.degrees( math.acos( np.dot(right_plane_normal_vec, right_wrist_finger_vector) / (math.sqrt( np.dot(right_wrist_finger_vector, right_wrist_finger_vector)) * math.sqrt( np.dot(right_plane_normal_vec, right_plane_normal_vec))))) else: right_side_bent_degree = 0 if left_plane_normal_vec[0] != 0 or left_plane_normal_vec[ 1] != 0 or left_plane_normal_vec[2] == 0: left_side_bent_degree = 90 - math.degrees( math.acos( np.dot(left_plane_normal_vec, left_wrist_finger_vector) / (math.sqrt( np.dot(left_wrist_finger_vector, left_wrist_finger_vector)) * math.sqrt( np.dot(left_plane_normal_vec, left_plane_normal_vec))))) else: left_side_bent_degree = 0 self.file.write("Right hand bending \n") self.file.write(str(right_side_bent_degree)) self.file.write("\n") self.file.write("Left hand bending \n") self.file.write(str(left_side_bent_degree)) self.file.write("\n")
def trunk_plane(self): m_body_number = bodyNum.BodyPartNumber() trunk_joint_numbers = m_body_number.trunk_whole_body() # finding a plane of upper body u = self.joints[trunk_joint_numbers[2]] - self.joints[trunk_joint_numbers[1]] v = self.joints[trunk_joint_numbers[2]] - self.joints[trunk_joint_numbers[0]] normal_plane = np.cross(u, v) return normal_plane
def neck_torsion_calculator(self): # TODO: I should calculate the rotation m_body_number = bodyNum.BodyPartNumber() neck_joint_numbers = m_body_number.neck() q1 = self.orientation[neck_joint_numbers[1]] q2 = self.orientation[neck_joint_numbers[0]] # finding the rotor that express rotation between two orientational frame(between outer and inner joint) rotor = util.find_rotation_quaternion(q1, q2) neck_twist = math.acos(rotor[0]) * 2 * (180 / np.pi) self.file.write("Neck twist \n") self.file.write(str(0)) self.file.write(str(neck_twist)) self.file.write("\n") return neck_twist
def trunk_torsion_calculator(self): # In here the rotor needed to transfer orientation frame of core joint to neck joint is calculated # this considered as twist m_body_number = bodyNum.BodyPartNumber() trunk_joint_numbers = m_body_number.trunk_whole_body() q1 = self.orientation[trunk_joint_numbers[2]] # neck q2 = self.orientation[trunk_joint_numbers[3]] # core # finding the rotor that express rotation between two orientational frame(between outer and inner joint) rotor = util.find_rotation_quaternion(q1, q2) trunk_twist = math.acos(rotor[0]) * 2 * (180 / np.pi) self.file.write("Trunk twist \n") self.file.write(str(0)) self.file.write("\n") return trunk_twist
def upper_arm_side_bending(self): m_body_number = bodyNum.BodyPartNumber() right_upper_arm_joint_numbers = m_body_number.right_arm() left_upper_arm_joint_numbers = m_body_number.left_arm() trunk_joint_numbers = m_body_number.trunk_upper_body() right_upper_arm_vector = self.joints[right_upper_arm_joint_numbers[ 2]] - self.joints[right_upper_arm_joint_numbers[1]] left_upper_arm_vector = self.joints[left_upper_arm_joint_numbers[ 2]] - self.joints[left_upper_arm_joint_numbers[1]] normal_trunk_plane = self.trunk_plane() proj_right_upperarm_on_plane = right_upper_arm_vector - np.dot( right_upper_arm_vector, normal_trunk_plane) * normal_trunk_plane proj_left_upperarm_on_plane = left_upper_arm_vector - np.dot( left_upper_arm_vector, normal_trunk_plane) * normal_trunk_plane spine_vector = self.joints[trunk_joint_numbers[0]] - self.joints[ trunk_joint_numbers[2]] right_side_degree = np.dot( spine_vector, proj_right_upperarm_on_plane) / ( math.sqrt(np.dot(spine_vector, spine_vector)) * math.sqrt( np.dot(proj_right_upperarm_on_plane, proj_right_upperarm_on_plane))) left_side_degree = np.dot( spine_vector, proj_left_upperarm_on_plane ) / (math.sqrt(np.dot(spine_vector, spine_vector)) * math.sqrt( np.dot(proj_left_upperarm_on_plane, proj_left_upperarm_on_plane))) if np.dot(np.cross(spine_vector, right_upper_arm_vector), normal_trunk_plane) < 0: # if the arm go to the body: adduction right_side_degree *= -1 if np.dot(np.cross(spine_vector, left_upper_arm_vector), normal_trunk_plane) > 0: left_side_degree *= -1 self.file.write("Upper right arm abduction \n") self.file.write(str(right_side_degree)) self.file.write("\n") self.file.write("Upper left arm abduction \n") self.file.write(str(left_side_degree)) self.file.write("\n")
def wrist_torsion(self): m_body_number = bodyNum.BodyPartNumber() right_wrist_joint_numbers = m_body_number.right_arm() q1 = self.orientation[right_wrist_joint_numbers[3]] q2 = self.orientation[right_wrist_joint_numbers[2]] # finding the rotor that express rotation between two orientational frame(between outer and inner joint) rotor = util.find_rotation_quaternion(q1, q2) right_wrist_twist = math.acos(rotor[0]) * 2 * (180 / np.pi) m_body_number = bodyNum.BodyPartNumber() left_wrist_joint_numbers = m_body_number.left_arm() q1 = self.orientation[left_wrist_joint_numbers[3]] q2 = self.orientation[left_wrist_joint_numbers[2]] # finding the rotor that express rotation between two orientational frame(between outer and inner joint) rotor = util.find_rotation_quaternion(q1, q2) left_wrist_twist = math.acos(rotor[0]) * 2 * (180 / np.pi) self.file.write("Right hand twist \n") self.file.write(str(right_wrist_twist)) self.file.write("\n") self.file.write("Left hand twist \n") self.file.write(str(left_wrist_twist)) self.file.write("\n")
def neck_flex_calculator(self): m_body_number = bodyNum.BodyPartNumber() neck_joint_numbers = m_body_number.neck() normal_plane = self.trunk_plane() neck_vector = self.joints[neck_joint_numbers[1]] - self.joints[ neck_joint_numbers[0]] neck_flex = 90 - math.degrees( math.acos( np.dot(neck_vector, normal_plane) / (math.sqrt(np.dot(normal_plane, normal_plane)) * math.sqrt(np.dot(neck_vector, neck_vector))))) self.file.write("Neck bending \n") self.file.write(str(neck_flex)) self.file.write("\n")
def trunk_side_calculator(self): m_body_number = bodyNum.BodyPartNumber() trunk_joint_numbers = m_body_number.trunk_whole_body() normal_plane_xz = np.array([0, 1, 0]) z_vector = np.array([0, 0, 1]) spine_vector = self.joints[trunk_joint_numbers[2]] - self.joints[trunk_joint_numbers[3]] project_spine_on_xz_plane = spine_vector - np.dot(spine_vector, normal_plane_xz) * normal_plane_xz trunk_side_bending = math.degrees(math.acos(np.dot(project_spine_on_xz_plane, z_vector) / ( math.sqrt(np.dot(project_spine_on_xz_plane, project_spine_on_xz_plane)) * math.sqrt( np.dot(z_vector, z_vector))))) self.file.write("Trunk side bending \n") self.file.write(str(trunk_side_bending)) self.file.write("\n")
def leg_degree(self): m_body_number = bodyNum.BodyPartNumber() right_leg_joint_numbers = m_body_number.right_leg() left_leg_joint_numbers = m_body_number.left_leg() right_knee_hip_vector = self.joints[right_leg_joint_numbers[ 0]] - self.joints[right_leg_joint_numbers[1]] right_ankle_knee_vector = self.joints[right_leg_joint_numbers[ 1]] - self.joints[right_leg_joint_numbers[2]] left_knee_hip_vector = self.joints[ left_leg_joint_numbers[0]] - self.joints[left_leg_joint_numbers[1]] left_ankle_knee_vector = self.joints[ left_leg_joint_numbers[1]] - self.joints[left_leg_joint_numbers[2]] # knee degree right_knee_degree = math.degrees( math.acos( np.dot(right_knee_hip_vector, right_ankle_knee_vector) / ((math.sqrt( np.dot(right_knee_hip_vector, right_knee_hip_vector))) * (math.sqrt( np.dot(right_ankle_knee_vector, right_ankle_knee_vector)))))) left_knee_degree = math.degrees( math.acos( np.dot(left_knee_hip_vector, left_ankle_knee_vector) / ((math.sqrt(np.dot(left_knee_hip_vector, left_knee_hip_vector))) * (math.sqrt( np.dot(left_ankle_knee_vector, left_ankle_knee_vector))))) ) self.file.write("Upper right leg flexion \n") self.file.write(str(right_knee_degree)) self.file.write("\n") self.file.write("Upper left leg flexion \n") self.file.write(str(left_knee_degree)) self.file.write("\n") # we consider both feet are fixed on ground self.file.write("Upper right leg abduction \n") self.file.write(str(0)) self.file.write("\n") self.file.write("Upper left leg abduction \n") self.file.write(str(0)) self.file.write("\n") self.file.write("Lower right leg flexion \n") self.file.write(str(0)) self.file.write("\n") self.file.write("Lower left leg flexion \n") self.file.write(str(0)) self.file.write("\n") self.file.write("Right foot flexion \n") self.file.write(str(0)) self.file.write("\n") self.file.write("Left foot flexion \n") self.file.write(str(0)) self.file.write("\n") self.file.write("Right foot bending \n") self.file.write(str(0)) self.file.write("\n") self.file.write("Left foot bending \n") self.file.write(str(0)) self.file.write("\n") # rotation should be written self.file.write("Right foot twist \n") self.file.write(str(0)) self.file.write("\n") self.file.write("Left foot twist \n") self.file.write(str(0)) self.file.write("\n") self.file.write("Upper right leg rotation \n") self.file.write(str(0)) self.file.write("\n") self.file.write("Upper left leg rotation \n") self.file.write(str(0)) self.file.write("\n")
def wrist_flex(self): m_body = bodyNum.BodyPartNumber() right_arm_joint_number = m_body.right_arm() left_arm_joint_number = m_body.left_arm() right_shoulder_elbow_vector = self.joints[ right_arm_joint_number[1]] - self.joints[right_arm_joint_number[0]] left_shoulder_elbow_vector = self.joints[ left_arm_joint_number[1]] - self.joints[left_arm_joint_number[0]] right_elbow_wrist_vector = self.joints[ right_arm_joint_number[2]] - self.joints[right_arm_joint_number[1]] left_elbow_wrist_vector = self.joints[ left_arm_joint_number[2]] - self.joints[left_arm_joint_number[1]] right_wrist_finger_vector = self.joints[ right_arm_joint_number[3]] - self.joints[right_arm_joint_number[2]] left_wrist_finger_vector = self.joints[ left_arm_joint_number[3]] - self.joints[left_arm_joint_number[2]] right_plane_normal_vec = np.cross(right_shoulder_elbow_vector, right_elbow_wrist_vector) left_plane_normal_vec = np.cross(left_shoulder_elbow_vector, left_elbow_wrist_vector) right_wrist_flex = math.degrees( math.acos( np.dot(right_elbow_wrist_vector, right_wrist_finger_vector) / (math.sqrt( np.dot(right_elbow_wrist_vector, right_elbow_wrist_vector)) * math.sqrt( np.dot(right_wrist_finger_vector, right_wrist_finger_vector))))) left_wrist_flex = math.degrees( math.acos( np.dot(left_elbow_wrist_vector, left_wrist_finger_vector) / (math.sqrt( np.dot(left_elbow_wrist_vector, left_elbow_wrist_vector)) * math.sqrt( np.dot(left_wrist_finger_vector, left_wrist_finger_vector))))) if right_plane_normal_vec[0] != 0 or right_plane_normal_vec[ 1] != 0 or right_plane_normal_vec[2] != 0: if np.dot( np.cross(right_wrist_finger_vector, right_elbow_wrist_vector), right_plane_normal_vec) > 0: # means extend of wrist right_wrist_flex *= -1 if left_plane_normal_vec[0] != 0 or left_plane_normal_vec[ 1] != 0 or left_plane_normal_vec[2] == 0: if np.dot( np.cross(left_wrist_finger_vector, left_elbow_wrist_vector), left_plane_normal_vec) > 0: # means extend of wrist left_wrist_flex *= -1 self.file.write("Right hand flexion \n") self.file.write(str(right_wrist_flex)) self.file.write("\n") self.file.write("Left hand flexion \n") self.file.write(str(left_wrist_flex)) self.file.write("\n")