def get_body_angles_from_pose_right(pose, verbose=False):
# type: (np.ndarray, bool) -> np.ndarray
'''
Get body angles from pose (look at right)
:param pose: Pose (Joints coordinates)
:param verbose: If true show each pose for debugging
:return: Body params (neck_angle, neck_side, trunk_angle, trunk_side,
legs_walking, legs_angle, load)
'''
pose = np.expand_dims(np.copy(pose), 0)
neck_angle, neck_side, trunk_angle, trunk_side, \
legs_walking, legs_angle, load = 0, 0, 0, 0, 0, 0, 0
if verbose:
utils.show_skeleton(pose, title="GT pose")
_pose, _ = utils.rotate_pose(np.copy(pose), rotation_joint=8)
_pose, _ = utils.rotate_pose(_pose, rotation_joint=8, m_coeff=np.pi)
utils.show_skeleton(_pose, title="GT pose Right")
pose, _ = utils.rotate_pose(pose, rotation_joint=8)
# Trunk position
pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi)
pose -= (pose[:, 8] + pose[:, 11]) / 2
trunk_angle = np.rad2deg((np.pi / 2) - np.arctan2(pose[0, 1, 1], pose[0, 1, 0]))
if verbose:
utils.show_skeleton(pose, title="Trunk angle: " + str(round(trunk_angle, 2)))
# Trunk bending
pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi / 2)
trunk_side_angle = abs(np.rad2deg(np.arctan2(pose[0, 1, 1], pose[0, 1, 0]) - (np.pi / 2)))
trunk_side = 1 if trunk_side_angle > 30 else 0
if verbose:
utils.show_skeleton(pose, title="Trunk side angle: " + str(round(trunk_side_angle, 2)))
# Neck position
pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=-np.pi / 2)
pose -= pose[:, 1]
neck_angle = np.rad2deg((np.pi / 2) - np.arctan2(pose[0, 0, 1], pose[0, 0, 0])) - trunk_angle
if verbose:
utils.show_skeleton(pose, title="Neck angle: " + str(round(neck_angle, 2)))
# Neck bending
pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi / 2)
neck_side_angle = np.abs(np.rad2deg(np.abs(np.arctan2(pose[0, 0, 1], pose[0, 0, 0])) - (np.pi / 2)))
neck_side = 1 if neck_side_angle > 20 else 0
if verbose:
utils.show_skeleton(pose, title="Neck side angle: " + str(round(neck_side_angle, 2)))
# Legs position
pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=-np.pi / 2)
pose -= pose[:, 11]
legs_angle = np.rad2deg((np.pi / 2) + np.arctan2(pose[0, 12, 1], pose[0, 12, 0]))
step_size = abs(np.linalg.norm(pose[0, 10, :2] - pose[0, 13, :2]))
legs_walking = 1 if step_size > 0.1 else 0
if verbose:
title = "Leg angle: " + str(round(legs_angle, 2)) + " Step size: " + str(round(step_size, 2))
utils.show_skeleton(pose, title=title)
return np.array([neck_angle, neck_side, trunk_angle, trunk_side,
legs_walking, legs_angle, load])
def get_arms_angles_from_pose_right(pose, verbose=False):
# type: (np.ndarray, bool) -> np.ndarray
'''
Get arms angles from pose (look at right)
:param pose: Pose (Joints coordinates)
:param verbose: If true show each pose for debugging
:return: Body params (upper_arm_angle, shoulder_raised, arm_abducted, leaning,
lower_arm_angle, wrist_angle, wrist_twisted)
'''
pose = np.expand_dims(np.copy(pose), 0)
if verbose:
utils.show_skeleton(pose, title="GT pose")
pose, _ = utils.rotate_pose(pose, rotation_joint=8)
pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi)
# Leaning
pose -= (pose[:, 8] + pose[:, 11]) / 2
trunk_angle = np.rad2deg((np.pi / 2) - np.arctan2(pose[0, 1, 1], pose[0, 1, 0]))
leaning = 1 if trunk_angle > 60 else 0
if verbose:
utils.show_skeleton(pose, title="Leaning angle: " + str(round(trunk_angle, 2)))
# Upper Arm position
pose -= pose[:, 5]
if quad(pose[0, 6]) == 2:
upper_arm_angle = -(270 - np.rad2deg(np.arctan2(pose[0, 6, 1], pose[0, 6, 0])) - trunk_angle)
else:
upper_arm_angle = np.rad2deg((np.pi /2) + np.arctan2(pose[0, 6, 1], pose[0, 6, 0])) + trunk_angle
if verbose:
utils.show_skeleton(pose, title="Upper Arms angle: " + str(round(upper_arm_angle, 2)))
# Upper Arm Adjust
pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi / 2)
shoulder_step = pose[:, 5, 1] - pose[:, 1, 1]
arm_abducted_angle = abs(np.rad2deg((np.pi / 2) + np.arctan2(pose[0, 6, 1], pose[0, 6, 0])))
shoulder_raised = 1 if shoulder_step > 0.02 else 0
arm_abducted = 1 if arm_abducted_angle > 45 else 0
if verbose:
print(shoulder_raised)
utils.show_skeleton(pose, title="Upper Arms abducted: " + str(round(arm_abducted_angle, 2)))
# Lower Arm position
pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=-np.pi / 2)
pose -= pose[:, 6]
lower_arm_angle = np.rad2deg((np.pi / 2) + np.arctan2(pose[0, 7, 1], pose[0, 7, 0]) ) + trunk_angle - upper_arm_angle
if verbose:
utils.show_skeleton(pose, title="Lower Arms angle: " + str(round(lower_arm_angle, 2)))
# Wrist position
wrist_angle = 0
wrist_twisted = 0
if pose.shape[1] > 14:
pose -= pose[:, 7]
wrist_angle = np.rad2deg((np.pi / 2) + np.arctan2(pose[0, 15, 1], pose[0, 15, 0]))
if verbose:
utils.show_skeleton(pose, title="Wrist Angle: " + str(round(wrist_angle, 2)))
pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=-np.pi / 2)
wrist_twisted_angle = abs(np.rad2deg((np.pi / 2) + np.arctan2(pose[0, 15, 1], pose[0, 15, 0])))
wrist_twisted = 1 if wrist_twisted_angle > 30 else 0
return np.array([upper_arm_angle, shoulder_raised, arm_abducted, leaning,
lower_arm_angle, wrist_angle, wrist_twisted])
def get_param_from_pose(pose, verbose=False):
# type: (np.ndarray, bool) -> np.ndarray
'''
Get body params from pose
:param pose: Pose (Joints coordinates)
:param verbose: If true show each pose for debugging
:return: Body params [trunk_angle, trunk_side_angle, arm_step_left, arm_step_right, legs_angle_left,
legs_angle_right, knee_angle_left, knee_angle_right, knee_offset_left,
knee_offset_right, step_size_y, step_size_x]
'''
pose = np.expand_dims(np.copy(pose), 0)
if verbose:
utils.show_skeleton(pose, title="GT pose")
# Trunk position
pose, _ = utils.rotate_pose(pose, rotation_joint=8)
pose -= (pose[:, 8] + pose[:, 11]) / 2
trunk_angle = np.rad2deg(np.arctan2(pose[0, 1, 1], pose[0, 1, 0]) - (np.pi / 2))
if verbose:
utils.show_skeleton(pose, title="Trunk angle: " +str(round(trunk_angle, 2)))
# Trunk twist
pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=np.pi/2)
trunk_side_angle = abs(np.rad2deg(np.arctan2(pose[0, 1, 1], pose[0, 1, 0]) - (np.pi / 2)))
if verbose:
utils.show_skeleton(pose, title="Trunk side angle: " + str(round(trunk_side_angle, 2)))
# Arms position
arm_step_left = pose[0, 3, 1] - pose[0, 2, 1]
arm_step_right = pose[0, 6, 1] - pose[0, 5, 1]
# Legs position
pose, _ = utils.rotate_pose(pose, rotation_joint=8, m_coeff=-np.pi / 2)
pose -= pose[:, 8]
legs_angle_left = -np.rad2deg(np.arctan2(pose[0, 9, 1], pose[0, 9, 0]) + (np.pi / 2))
pose -= pose[:, 11]
legs_angle_right = -np.rad2deg(np.arctan2(pose[0, 12, 1], pose[0, 12, 0]) + (np.pi / 2))
if verbose:
utils.show_skeleton(pose, title="Legs angle left: " + str(round(legs_angle_left, 2))
+ " Legs angle right: " + str(round(legs_angle_right, 2)))
pose -= pose[:, 9]
a = np.rad2deg(np.arctan2(pose[0, 10, 1], pose[0, 10, 0]))
b = np.rad2deg(np.arctan2(pose[0, 8, 1], pose[0, 8, 0]))
knee_angle_left = abs(a) + abs(b)
pose -= pose[:, 12]
a = np.rad2deg(np.arctan2(pose[0, 13, 1], pose[0, 13, 0]))
b = np.rad2deg(np.arctan2(pose[0, 11, 1], pose[0, 11, 0]))
knee_angle_right = abs(a) + abs(b)
if verbose:
utils.show_skeleton(pose, title="Knee angle left: " + str(round(knee_angle_left, 2))
+ " Knee angle right: " + str(round(knee_angle_right, 2)))
knee_offset_left = abs(pose[0, 9, 1] - pose[0, 10, 1])
knee_offset_right = abs(pose[0, 12, 1] - pose[0, 13, 1])
step_size_y = abs(pose[0, 10, 1] - pose[0, 13, 1])
step_size_x = abs(pose[0, 10, 0] - pose[0, 13, 0])
return np.array([trunk_angle, trunk_side_angle, arm_step_left, arm_step_right, legs_angle_left,
legs_angle_right, knee_angle_left, knee_angle_right, knee_offset_left,
knee_offset_right, step_size_y, step_size_x])