def move_look_trajectory(task, trajectory, max_tilt=np.pi / 6): # max_tilt=INF): # TODO: implement a minimum distance instead of max_tilt # TODO: pr2 movement restrictions #base_path = [pose.to_base_conf() for pose in trajectory.path] base_path = trajectory.path if not base_path: return trajectory obstacles = get_fixed_bodies(task) # TODO: movable objects robot = base_path[0].body target_path = get_target_path(trajectory) waypoints = [] index = 0 with BodySaver(robot): #current_conf = base_values_from_pose(get_pose(robot)) for i, conf in enumerate(base_path): # TODO: just do two loops? conf.assign() while index < len(target_path): if i < index: # Don't look at past or current conf target_point = target_path[index] head_conf = inverse_visibility( robot, target_point) # TODO: this is slightly slow #print(index, target_point, head_conf) if (head_conf is not None) and (head_conf[1] < max_tilt): break index += 1 else: head_conf = get_group_conf(robot, 'head') set_group_conf(robot, 'head', head_conf) #print(i, index, conf.values, head_conf) #, get_pose(robot)) waypoints.append(np.concatenate([conf.values, head_conf])) joints = tuple(base_path[0].joints) + tuple(get_group_joints( robot, 'head')) #joints = get_group_joints(robot, 'base') + get_group_joints(robot, 'head') #set_pose(robot, unit_pose()) #set_group_conf(robot, 'base', current_conf) path = plan_waypoints_joint_motion(robot, joints, waypoints, obstacles=obstacles, self_collisions=SELF_COLLISIONS) return create_trajectory(robot, joints, path)
def inspect_trajectory(trajectory): if not trajectory.path: return robot = trajectory.path[0].body # TODO: minimum distance of some sort (to prevent from looking at the bottom) # TODO: custom lower limit as well head_waypoints = [] for target_point in get_target_path(trajectory): head_conf = inverse_visibility(robot, target_point) # TODO: could also draw the sequence of inspected points as the head moves if head_conf is None: continue head_waypoints.append(head_conf) head_joints = get_group_joints(robot, 'head') #return create_trajectory(robot, head_joints, head_waypoints) head_path = plan_waypoints_joint_motion(robot, head_joints, head_waypoints, obstacles=None, self_collisions=False) assert (head_path is not None) return create_trajectory(robot, head_joints, head_path)