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