def plan_head_traj(robot, head_conf):
# head_conf = get_joint_positions(robot, head_joints)
# head_path = [head_conf, hq.values]
head_joints = get_group_joints(robot, 'head')
head_path = plan_direct_joint_motion(robot,
head_joints,
head_conf,
obstacles=None,
self_collisions=False)
assert (head_path is not None)
return create_trajectory(robot, head_joints, head_path)
def plan_head_traj(task, head_conf):
robot = task.robot
obstacles = get_fixed_bodies(task) # TODO: movable objects
# head_conf = get_joint_positions(robot, head_joints)
# head_path = [head_conf, hq.values]
head_joints = get_group_joints(robot, 'head')
head_path = plan_direct_joint_motion(robot,
head_joints,
head_conf,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS)
assert (head_path is not None)
return create_trajectory(robot, head_joints, head_path)
def test(hq1, hq2):
if teleport:
ht = Trajectory([hq1, hq2])
else:
hq1.assign()
path = plan_direct_joint_motion(rover,
hq1.joints,
hq2.values,
self_collisions=False)
if path is None:
return None
ht = create_trajectory(rover, hq1.joints, path)
return (ht, )
def test(rover, q1, q2, fluents=[]):
if teleport:
ht = Trajectory([q1, q2])
return Output(ht)
base_link = child_link_from_joint(q1.joints[-1])
q1.assign()
attachments = []
movable = set()
for fluent in fluents:
predicate, args = fluent[0], fluent[1:]
if predicate == 'AtGrasp'.lower():
r, b, g = args
attachments.append(Attachment(rover, base_link, g.value, b))
elif predicate == 'AtPose'.lower():
b, p = args
assert b not in movable
p.assign()
movable.add(b)
# elif predicate == 'AtConf'.lower():
# continue
else:
raise NotImplementedError(predicate)
obstacles = set(problem.fixed) | movable if collisions else []
q1.assign()
if holonomic:
path = plan_joint_motion(rover,
q1.joints,
q2.values,
custom_limits=custom_limits,
attachments=attachments,
obstacles=obstacles,
self_collisions=False,
**kwargs)
else:
path = plan_nonholonomic_motion(rover,
q1.joints,
q2.values,
reversible=reversible,
custom_limits=custom_limits,
attachments=attachments,
obstacles=obstacles,
self_collisions=False,
**kwargs)
if path is None:
return None
ht = create_trajectory(rover, q2.joints, path)
return Output(ht)
def test(bq1, bq2):
if teleport:
ht = Trajectory([bq1, bq2])
else:
bq1.assign()
path = plan_joint_motion(rover,
bq1.joints,
bq2.values,
custom_limits=custom_limits,
obstacles=fixed,
self_collisions=False)
if path is None:
return None
ht = create_trajectory(rover, bq2.joints, path)
return (ht, )
def test(rover, q1, q2):
if teleport:
ht = Trajectory([q1, q2])
else:
q1.assign()
path = plan_nonholonomic_motion(rover,
q1.joints,
q2.values,
custom_limits=custom_limits,
obstacles=obstacles,
self_collisions=False,
**kwargs)
if path is None:
return None
ht = create_trajectory(rover, q2.joints, path)
return (ht, )
def fn(body, q1, q2):
joints = get_base_joints(body)
distance_fn = get_base_distance_fn(weights)
extend_fn = get_extend_fn(body, joints, resolutions=resolutions)
collision_fn = get_collision_fn(body, joints, problem.obstacles, attachments=[],
self_collisions=False, disabled_collisions=set(),
custom_limits=problem.custom_limits, max_distance=MAX_DISTANCE)
# TODO: degree
distance = distance_fn(q1.values, q2.values)
if max_distance < distance:
return None
path = [q1.values] + list(extend_fn(q1.values, q2.values))
if any(map(collision_fn, path)):
return None
handles = add_line(point_from_conf(q1.values), point_from_conf(q2.values))
t = create_trajectory(body, joints, path)
return (t,)
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 gen(o, p):
if o in task.rooms:
#bq = Pose(robot, unit_pose())
#bq = state.poses[robot]
bq = Conf(robot, base_joints, np.zeros(len(base_joints)))
#hq = Conf(robot, head_joints, np.zeros(len(head_joints)))
#ht = create_trajectory(robot, head_joints, plan_pause_scan_path(robot, tilt=tilt))
waypoints = plan_scan_path(task.robot, tilt=tilt)
set_group_conf(robot, 'head', waypoints[0])
path = plan_waypoints_joint_motion(robot,
head_joints,
waypoints[1:],
obstacles=None,
self_collisions=False)
ht = create_trajectory(robot, head_joints, path)
yield bq, ht.path[0], ht
else:
for bq, hq in vis_gen(o, p):
ht = Trajectory([hq])
yield bq, ht.path[0], ht
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)
def fn(o, p, bq):
set_pose(o, p.value) # p.assign()
bq.assign()
if o in task.rooms:
waypoints = plan_scan_path(task.robot, tilt=ROOM_SCAN_TILT)
set_group_conf(robot, 'head', waypoints[0])
path = plan_waypoints_joint_motion(robot,
head_joints,
waypoints[1:],
obstacles=None,
self_collisions=False)
if path is None:
return None
ht = create_trajectory(robot, head_joints, path)
hq = ht.path[0]
else:
target_point = point_from_pose(p.value)
head_conf = inverse_visibility(robot, target_point)
if head_conf is None: # TODO: test if visible
return None
hq = Conf(robot, head_joints, head_conf)
ht = Trajectory([hq])
return (hq, ht)
def linear_trajectory(conf1, conf2):
robot = conf1.body
joints = conf1.joints
extend_fn = get_extend_fn(robot, joints, resolutions=BASE_RESOLUTIONS)
path = [conf1.values] + list(extend_fn(conf1.values, conf2.values))
return create_trajectory(robot, joints, path)
