def pddlstream_from_problem(robot, movable=[], teleport=False, grasp_name='top'): #assert (not are_colliding(tree, kin_cache)) domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} print('Robot:', robot) conf = BodyConf(robot, get_configuration(robot)) init = [('CanMove',), ('Conf', conf), ('AtConf', conf), ('HandEmpty',)] fixed = get_fixed(robot, movable) print('Movable:', movable) print('Fixed:', fixed) for body in movable: pose = BodyPose(body, get_pose(body)) init += [('Graspable', body), ('Pose', body, pose), ('AtPose', body, pose)] for surface in fixed: init += [('Stackable', body, surface)] if is_placement(body, surface): init += [('Supported', body, pose, surface)] for body in fixed: name = get_body_name(body) if 'sink' in name: init += [('Sink', body)] if 'stove' in name: init += [('Stove', body)] body = movable[0] goal = ('and', ('AtConf', conf), #('Holding', body), #('On', body, fixed[1]), #('On', body, fixed[2]), #('Cleaned', body), ('Cooked', body), ) stream_map = { 'sample-pose': from_gen_fn(get_stable_gen(fixed)), 'sample-grasp': from_gen_fn(get_grasp_gen(robot, grasp_name)), 'inverse-kinematics': from_fn(get_ik_fn(robot, fixed, teleport)), 'plan-free-motion': from_fn(get_free_motion_gen(robot, fixed, teleport)), 'plan-holding-motion': from_fn(get_holding_motion_gen(robot, fixed, teleport)), 'TrajCollision': get_movable_collision_test(), } if USE_SYNTHESIZERS: stream_map.update({ 'plan-free-motion': empty_gen(), 'plan-holding-motion': empty_gen(), }) return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def plan_commands(state, viewer=False, teleport=False, profile=False, verbose=True): # TODO: could make indices into set of bodies to ensure the same... # TODO: populate the bodies here from state and not the real world sim_world = connect(use_gui=viewer) #clone_world(client=sim_world) task = state.task robot_conf = get_configuration(task.robot) robot_pose = get_pose(task.robot) with ClientSaver(sim_world): with HideOutput(): robot = create_pr2(use_drake=USE_DRAKE_PR2) set_pose(robot, robot_pose) set_configuration(robot, robot_conf) mapping = clone_world(client=sim_world, exclude=[task.robot]) assert all(i1 == i2 for i1, i2 in mapping.items()) set_client(sim_world) saved_world = WorldSaver() # StateSaver() pddlstream_problem = pddlstream_from_state(state, teleport=teleport) _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', sorted(init, key=lambda f: f[0])) if verbose: print('Goal:', goal) print('Streams:', stream_map.keys()) stream_info = { 'test-vis-base': StreamInfo(eager=True, p_success=0), 'test-reg-base': StreamInfo(eager=True, p_success=0), } hierarchy = [ ABSTRIPSLayer(pos_pre=['AtBConf']), ] pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, stream_info=stream_info, hierarchy=hierarchy, debug=False, success_cost=MAX_COST, verbose=verbose) plan, cost, evaluations = solution if MAX_COST <= cost: plan = None print_solution(solution) print('Finite cost:', cost < MAX_COST) commands = post_process(state, plan) pr.disable() if profile: pstats.Stats(pr).sort_stats('cumtime').print_stats(10) saved_world.restore() disconnect() return commands
def plan_commands(state, teleport=False, profile=False, verbose=True): # TODO: could make indices into set of bodies to ensure the same... # TODO: populate the bodies here from state task = state.task robot_conf = get_configuration(task.robot) robot_pose = get_pose(task.robot) sim_world = connect(use_gui=False) with ClientSaver(sim_world): with HideOutput(): robot = create_pr2(use_drake=USE_DRAKE_PR2) #robot = load_model(DRAKE_PR2_URDF, fixed_base=True) set_pose(robot, robot_pose) set_configuration(robot, robot_conf) clone_world(client=sim_world, exclude=[task.robot]) set_client(sim_world) saved_world = WorldSaver() # StateSaver() pddlstream_problem = pddlstream_from_state(state, teleport=teleport) _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', sorted(init, key=lambda f: f[0])) if verbose: print('Goal:', goal) print('Streams:', stream_map.keys()) stream_info = { 'test-vis-base': StreamInfo(eager=True, p_success=0), 'test-reg-base': StreamInfo(eager=True, p_success=0), } pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, stream_info=stream_info, max_cost=MAX_COST, verbose=verbose) pr.disable() plan, cost, evaluations = solution if MAX_COST <= cost: plan = None print_solution(solution) print('Finite cost:', cost < MAX_COST) print('Real cost:', float(cost) / SCALE_COST) if profile: pstats.Stats(pr).sort_stats('tottime').print_stats(10) saved_world.restore() commands = post_process(state, plan) disconnect() return commands
def get_pddlstream(robot, brick_from_index, obstacle_from_name, collisions=True, teleport=False): domain_pddl = read( get_file_path(__file__, os.path.join(PICKNPLACE_DIRECTORY, 'domain.pddl'))) stream_pddl = read( get_file_path(__file__, os.path.join(PICKNPLACE_DIRECTORY, 'stream.pddl'))) constant_map = {} conf = np.array(get_configuration(robot)) init = [ ('CanMove', ), ('Conf', conf), ('AtConf', conf), ('HandEmpty', ), ] goal_literals = [ ('AtConf', conf), #('HandEmpty',), ] #indices = brick_from_index.keys() #indices = range(2, 5) indices = [4] for index in list(indices): indices.append(index + 6) for index in indices: brick = brick_from_index[index] init += [ ('Graspable', index), ('Pose', index, brick.initial_pose), ('AtPose', index, brick.initial_pose), ('Pose', index, brick.goal_pose), ] goal_literals += [ #('Holding', index), ('AtPose', index, brick.goal_pose), ] for index2 in brick.goal_supports: brick2 = brick_from_index[index2] init += [ ('Supported', index, brick.goal_pose, index2, brick2.goal_pose), ] goal = And(*goal_literals) if not collisions: obstacle_from_name = {} stream_map = { 'sample-grasp': from_gen_fn(get_grasp_gen_fn(brick_from_index)), 'inverse-kinematics': from_gen_fn(get_ik_gen_fn(robot, brick_from_index, obstacle_from_name)), 'plan-motion': from_fn( get_motion_fn(robot, brick_from_index, obstacle_from_name, teleport=teleport)), 'TrajCollision': get_collision_test(robot, brick_from_index, collisions=collisions), } #stream_map = 'debug' return domain_pddl, constant_map, stream_pddl, stream_map, init, goal