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 trunk_plane(self):
m_body_number = bodyNum.body_part_number()
trunk_joint_numbers = m_body_number.trunk_upper_body()
# finding a plane of upper body
u = self.joints[trunk_joint_numbers[1]] - self.joints[
trunk_joint_numbers[0]]
v = self.joints[trunk_joint_numbers[3]] - self.joints[
trunk_joint_numbers[0]]
normal_plane = np.cross(u, v)
return normal_plane
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 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 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 trunk_side_calculator(self):
m_body_number = bodyNum.body_part_number()
trunk_joint_numbers = m_body_number.trunk_whole_body()
normal_plane_xy = np.array([0, 0, 1])
y_vector = np.array([0, 1, 0])
spine_vector = self.joints_position[trunk_joint_numbers[
2]] - self.joints_position[trunk_joint_numbers[3]]
project_spine_on_xy_plane = spine_vector - np.dot(
spine_vector, normal_plane_xy) * normal_plane_xy
trunk_side_bending = math.degrees(
math.acos(
np.dot(project_spine_on_xy_plane, y_vector) /
(math.sqrt(
np.dot(project_spine_on_xy_plane,
project_spine_on_xy_plane)) *
math.sqrt(np.dot(y_vector, y_vector)))))
return trunk_side_bending
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 neck_side_calculator(self):
m_body_number = bodyNum.body_part_number()
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)))))
return neck_side_bending
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]
