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