def get_left_arm(kinect_pos, kinect_rot, must_filter=True):
# Get the conversion matrix and the axes in the "robot world"
world = get_robot_world(kinect_pos)
# Compute the angle for every part of the arm
shoulder_roll = utils.valid_angle(get_left_shoulder_roll(kinect_pos, world=world) * 180 / np.pi)
shoulder_pitch = utils.valid_angle(get_left_shoulder_pitch(kinect_pos, world=world) * 180 / np.pi)
elbow_roll = utils.valid_angle(get_left_elbow_roll(kinect_pos, world=world) * 180 / np.pi)
elbow_yaw = utils.valid_angle(get_left_elbow_yaw(kinect_pos,
shoulder_pitch=shoulder_pitch * np.pi / 180,
shoulder_roll=shoulder_roll * np.pi / 180,
world=world) * 180 / np.pi)
# Is the elbow angle enough to bend it on the robot ?
if elbow_roll < -50:
elbow_yaw -= shoulder_pitch + 15
# Else, flat arm
else:
elbow_yaw = -80
elbow_roll = -7.5
elbow_yaw = max(-110, elbow_yaw)
elbow_yaw = min(110, elbow_yaw)
# Compute Wrist yaw given the other angles + rotation list
wrist_yaw = get_left_wrist_yaw(kinect_rot, elbow_yaw)
if must_filter:
shoulder_roll = utils.value_filter("l_s_roll", shoulder_roll)
shoulder_pitch = utils.value_filter("l_s_pitch", shoulder_pitch)
elbow_roll = utils.value_filter("l_e_roll", elbow_roll)
elbow_yaw = utils.value_filter("l_e_yaw", elbow_yaw)
wrist_yaw = utils.value_filter("l_w_yaw", wrist_yaw)
return [shoulder_roll, shoulder_pitch, elbow_roll, elbow_yaw, wrist_yaw]
def get_head_pitch(kinect_pos, world=None, must_filter=True):
if world is None:
world = get_robot_world(kinect_pos)
head = kinect_pos[kinecthandler.joints_map[joints.HEAD]]
neck = kinect_pos[kinecthandler.joints_map[joints.NECK]]
modified_neck = [neck[0], neck[1] - 1, neck[2]]
neck_head = utils.get_vector(head, neck, transform=world[0])
modified_neck_head = utils.get_vector(head, modified_neck, transform=world[0])
res = np.arccos(utils.normalized_dot(neck_head, modified_neck_head))
sign = 1
if head[2] > neck[2]:
sign = -1
res *= sign
res = max(res, -0.66)
res = min(res, 0.5)
if must_filter:
res = utils.value_filter("h_pitch", res)
return res
