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)
