def test(bq, o2, wp2):
if not collisions:
return True
if isinstance(wp2, SurfaceDist):
return True # TODO: perform this probabilistically
bq.assign()
world.carry_conf.assign()
wp2.assign()
obstacles = get_link_obstacles(world, o2)
return not any(
pairwise_collision(world.robot, obst) for obst in obstacles)
def parse_fluents(world, fluents):
obstacles = set()
for fluent in fluents:
predicate, args = fluent[0], fluent[1:]
if predicate in {p.lower() for p in ['AtBConf', 'AtAConf', 'AtGConf']}:
q, = args
q.assign()
elif predicate == 'AtAngle'.lower():
j, a = args
a.assign()
link = child_link_from_joint(a.joints[0])
obstacles.update(get_descendant_obstacles(a.body, link))
elif predicate in 'AtWorldPose'.lower():
# TODO: conditional effects are not being correctly updated in pddlstream
#b, p = args
#if isinstance(p, SurfaceDist):
# continue
#p.assign()
#obstacles.update(get_link_obstacles(world, b))
raise RuntimeError()
elif predicate in 'AtRelPose'.lower():
pass
elif predicate == 'AtGrasp'.lower():
pass
else:
raise NotImplementedError(predicate)
attachments = []
for fluent in fluents:
predicate, args = fluent[0], fluent[1:]
if predicate in {p.lower() for p in ['AtBConf', 'AtAConf', 'AtGConf']}:
pass
elif predicate == 'AtAngle'.lower():
pass
elif predicate in 'AtWorldPose'.lower():
raise RuntimeError()
elif predicate in 'AtRelPose'.lower():
o1, rp, o2 = args
if isinstance(rp, SurfaceDist):
continue
rp.assign()
obstacles.update(get_link_obstacles(world, o1))
elif predicate == 'AtGrasp'.lower():
o, g = args
if o is not None:
attachments.append(g.get_attachment())
attachments[-1].assign()
else:
raise NotImplementedError(predicate)
return attachments, obstacles
def test(detect, obj_name, pose):
if (detect.name == obj_name) or (detect.surface_name
== obj_name) or isinstance(
pose, SurfaceDist):
return True
move_occluding(world)
detect.pose.assign()
pose.assign()
body = world.get_body(detect.name)
obstacles = get_link_obstacles(world, obj_name)
if any(pairwise_collision(body, obst) for obst in obstacles):
return False
visible = not obstacles & detect.compute_occluding()
#if not visible:
# handles = detect.draw()
# wait_for_user()
# remove_handles(handles)
return visible
def test(detect, bconf, aconf, obj_name, grasp):
if detect.name == obj_name:
return True
# TODO: check collisions with the placement distribution
# Move top grasps more vertically
move_occluding(world)
bconf.assign()
aconf.assign()
detect.pose.assign()
if obj_name is not None:
grasp.assign()
obstacles = get_link_obstacles(world, obj_name)
else:
obstacles = get_descendant_obstacles(world.robot)
visible = not obstacles & detect.compute_occluding()
#if not visible:
# handles = detect.draw()
# wait_for_user()
# remove_handles(handles)
return visible
def test(j1, a1, a2, o2, wp):
if not collisions or (o2 in j1): # (j1 == JOINT_TEMPLATE.format(o2)):
return True
# TODO: check pregrasp path as well
# TODO: pull path collisions
wp.assign()
set_configuration(world.gripper, world.open_gq.values)
status = compute_door_paths(world,
j1,
a1,
a2,
obstacles=get_link_obstacles(world, o2))
#print(j1, a1, a2, o2, wp)
#if not status:
# set_renderer(enable=True)
# for a in [a1, a2]:
# a.assign()
# wait_for_user()
# set_renderer(enable=False)
return status
def test(o1, wp1, g1, o2, wp2):
# o1 will always be a movable object
if isinstance(wp2, SurfaceDist):
return True # TODO: perform this probabilistically
if not collisions or (o1 == o2) or (o2 == wp1.support):
return True
# TODO: could define these on sets of samples to prune all at once
body = world.get_body(o1)
wp2.assign()
obstacles = get_link_obstacles(world, o2) # - {body}
if not obstacles:
return True
for _ in iterate_approach_path(world, wp1, g1, body=body):
if any(
pairwise_collision(part, obst)
for part in [world.gripper, body] for obst in obstacles):
# TODO: some collisions the bottom drawer and the top drawer handle
#print(o1, wp1.support, o2, wp2.support)
#wait_for_user()
return False
return True
def test(at, o, wp):
if not collisions:
return True
# TODO: check door collisions
# TODO: still need to check static links at least once
if isinstance(wp, SurfaceDist):
return True # TODO: perform this probabilistically
wp.assign()
state = at.context.copy()
state.assign()
all_bodies = {
body
for command in at.commands for body in command.bodies
}
for command in at.commands:
obstacles = get_link_obstacles(world, o) - all_bodies
# TODO: why did I previously remove o at p?
#obstacles = get_link_obstacles(world, o) - command.bodies # - p.bodies # Doesn't include o at p
if not obstacles:
continue
if isinstance(command, DoorTrajectory):
[door_joint] = command.door_joints
surface_name = get_link_name(world.kitchen,
child_link_from_joint(door_joint))
if wp.support == surface_name:
return True
for _ in command.iterate(state):
state.derive()
#for attachment in state.attachments.values():
# if any(pairwise_collision(attachment.child, obst) for obst in obstacles):
# return False
# TODO: just check collisions with moving links
if any(
pairwise_collision(world.robot, obst)
for obst in obstacles):
#print(at, o, p)
#wait_for_user()
return False
return True