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