def wrist_flex(self): m_body = bodyNum.body_part_number() right_arm_joint_number = m_body.right_arm() left_arm_joint_number = m_body.left_arm() right_shoulder_elbow_vector = self.joints_position[ right_arm_joint_number[1]] - self.joints_position[ right_arm_joint_number[0]] left_shoulder_elbow_vector = self.joints_position[ left_arm_joint_number[1]] - self.joints_position[ left_arm_joint_number[0]] right_elbow_wrist_vector = self.joints_position[right_arm_joint_number[ 2]] - self.joints_position[right_arm_joint_number[1]] left_elbow_wrist_vector = self.joints_position[left_arm_joint_number[ 2]] - self.joints_position[left_arm_joint_number[1]] right_wrist_finger_vector = self.joints_position[ right_arm_joint_number[3]] - self.joints_position[ right_arm_joint_number[2]] left_wrist_finger_vector = self.joints_position[left_arm_joint_number[ 3]] - self.joints_position[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 = Util.get_angle_between_degs( right_elbow_wrist_vector, right_wrist_finger_vector) left_wrist_flex = Util.get_angle_between_degs( left_elbow_wrist_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 return [right_wrist_flex, left_wrist_flex]
def leg_degree(self): m_body_number = bodyNum.body_part_number() right_leg_joint_numbers = m_body_number.right_leg() left_leg_joint_numbers = m_body_number.left_leg() right_knee_hip_vector = self.joints_position[right_leg_joint_numbers[0]] - self.joints_position[right_leg_joint_numbers[1]] right_ankle_knee_vector = self.joints_position[right_leg_joint_numbers[1]] - self.joints_position[right_leg_joint_numbers[2]] left_knee_hip_vector = self.joints_position[left_leg_joint_numbers[0]] - self.joints_position[left_leg_joint_numbers[1]] left_ankle_knee_vector = self.joints_position[left_leg_joint_numbers[1]] - self.joints_position[left_leg_joint_numbers[2]] # knee degree right_knee_degree = Util.get_angle_between_degs(right_knee_hip_vector, right_ankle_knee_vector) left_knee_degree = Util.get_angle_between_degs(left_knee_hip_vector, left_ankle_knee_vector) return [right_knee_degree, left_knee_degree]
def lower_arm_degree(self): m_body_number = bodyNum.body_part_number() right_arm_joint_numbers = m_body_number.right_arm() left_arm_joint_numbers = m_body_number.left_arm() right_shoulder_elbow_vector = self.joints_position[right_arm_joint_numbers[1]] - self.joints_position[right_arm_joint_numbers[0]] left_shoulder_elbow_vector = self.joints_position[left_arm_joint_numbers[1]] - self.joints_position[left_arm_joint_numbers[0]] right_elbow_wrist_vector = self.joints_position[right_arm_joint_numbers[2]] - self.joints_position[right_arm_joint_numbers[1]] left_elbow_wrist_vector = self.joints_position[left_arm_joint_numbers[2]] - self.joints_position[left_arm_joint_numbers[1]] # right and left arm degree in saggital plane right_degree = Util.get_angle_between_degs(right_shoulder_elbow_vector, right_elbow_wrist_vector) left_degree = Util.get_angle_between_degs(left_shoulder_elbow_vector, left_elbow_wrist_vector) return [right_degree,left_degree]
def neck_twist_calculator(self): m_body_number = bodyNum.body_part_number() 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) return neck_twist
def neck_flex_calculator(self): m_body_number = bodyNum.body_part_number() 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 - Util.get_angle_between_degs(neck_vector, normal_plane) return neck_flex
def trunk_twist_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.body_part_number() trunk_joint_numbers = m_body_number.trunk_whole_body() q1 = self.joints_orientation[trunk_joint_numbers[2]] # neck q2 = self.joints_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) return trunk_twist
def shoulder_rise(self): m_body_number = bodyNum.body_part_number() 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_position[trunk_joint_numbers[ 0]] - self.joints_position[trunk_joint_numbers[2]] right_shoulder_vector = self.joints_position[ right_shoulder_joint_numbers[1]] - self.joints_position[ right_shoulder_joint_numbers[0]] left_shoulder_vector = self.joints_position[ left_shoulder_joint_numbers[1]] - self.joints_position[ left_shoulder_joint_numbers[0]] right_shoulder_rise_degree = 90 - Util.get_angle_between_degs( spine_vector, right_shoulder_vector) left_shoulder_rise_degree = 90 - Util.get_angle_between_degs( spine_vector, left_shoulder_vector) return [right_shoulder_rise_degree, left_shoulder_rise_degree]
def wrist_side(self): m_body = bodyNum.body_part_number() right_arm_joint_number = m_body.right_arm() left_arm_joint_number = m_body.left_arm() right_shoulder_elbow_vector = self.joints_position[ right_arm_joint_number[1]] - self.joints_position[ right_arm_joint_number[0]] left_shoulder_elbow_vector = self.joints_position[ left_arm_joint_number[1]] - self.joints_position[ left_arm_joint_number[0]] right_elbow_wrist_vector = self.joints_position[right_arm_joint_number[ 2]] - self.joints_position[right_arm_joint_number[1]] left_elbow_wrist_vector = self.joints_position[left_arm_joint_number[ 2]] - self.joints_position[left_arm_joint_number[1]] right_wrist_finger_vector = self.joints_position[ right_arm_joint_number[3]] - self.joints_position[ right_arm_joint_number[2]] left_wrist_finger_vector = self.joints_position[left_arm_joint_number[ 3]] - self.joints_position[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 - Util.get_angle_between_degs( right_plane_normal_vec, right_wrist_finger_vector) 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 - Util.get_angle_between_degs( left_plane_normal_vec, left_wrist_finger_vector) else: left_side_bent_degree = 0 return [right_side_bent_degree, left_side_bent_degree]
def upper_arm_side_bending(self): m_body_number = bodyNum.body_part_number() 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_position[ right_upper_arm_joint_numbers[2]] - self.joints_position[ right_upper_arm_joint_numbers[1]] left_upper_arm_vector = self.joints_position[ left_upper_arm_joint_numbers[2]] - self.joints_position[ 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_position[trunk_joint_numbers[ 0]] - self.joints_position[trunk_joint_numbers[2]] right_side_degree = Util.get_angle_between_degs( spine_vector, proj_right_upperarm_on_plane) left_side_degree = Util.get_angle_between_degs( spine_vector, 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 return [right_side_degree, left_side_degree]
def wrist_torsion(self): m_body_number = bodyNum.body_part_number() right_wrist_joint_numbers = m_body_number.right_arm() q1 = self.joints_orientation[right_wrist_joint_numbers[3]] q2 = self.joints_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) if (rotor[0] > 1): rotor[0] = 1 elif rotor[0] < -1: rotor[0] = -1 right_wrist_twist = math.acos(rotor[0]) * 2 * (180 / np.pi) m_body_number = bodyNum.body_part_number() left_wrist_joint_numbers = m_body_number.left_arm() q1 = self.joints_orientation[left_wrist_joint_numbers[3]] q2 = self.joints_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) return [right_wrist_twist, left_wrist_twist]
def upper_arm_flex(self): m_body_number = bodyNum.body_part_number() 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_whole_body() right_upper_arm_vector = self.joints_position[ right_upper_arm_joint_numbers[1]] - self.joints_position[ right_upper_arm_joint_numbers[0]] left_upper_arm_vector = self.joints_position[ left_upper_arm_joint_numbers[1]] - self.joints_position[ left_upper_arm_joint_numbers[0]] # normal_trunk_plane = self.trunk_plane() spine_vector = self.joints_position[trunk_joint_numbers[ 3]] - self.joints_position[trunk_joint_numbers[2]] flex_right_upper_arm = Util.get_angle_between_degs( right_upper_arm_vector, spine_vector) flex_left_upper_arm = Util.get_angle_between_degs( left_upper_arm_vector, spine_vector) return [flex_right_upper_arm, flex_left_upper_arm]
def trunk_flex_calculator(self): normal_plane = self.trunk_plane() y_vector = np.array([0, 1, 0]) trunk_flex = Util.get_angle_between_degs(y_vector, normal_plane) - 90 return trunk_flex