def main(focused=True, unit_costs=False): problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem tamp_problem = problem_fn() print(tamp_problem) pddlstream_problem = pddlstream_from_tamp(tamp_problem) if focused: solution = solve_focused(pddlstream_problem, unit_costs=unit_costs) else: #solution = solve_exhaustive(pddlstream_problem, unit_costs=unit_costs) solution = solve_incremental(pddlstream_problem, unit_costs=unit_costs) print_solution(solution) plan, cost, evaluations = solution if plan is None: return print(evaluations) colors = dict(zip(tamp_problem.initial.block_poses, COLORS)) viewer = DiscreteTAMPViewer(1, len(tamp_problem.poses), title='Initial') state = tamp_problem.initial print(state) draw_state(viewer, state, colors) for action in plan: user_input('Continue?') state = apply_action(state, action) print(state) draw_state(viewer, state, colors) user_input('Finish?')
def main(): parser = argparse.ArgumentParser() parser.add_argument('-a', '--algorithm', default='incremental', help='Specifies the algorithm') args = parser.parse_args() print('Arguments:', args) problem_fn = get_problem # get_problem1 | get_problem2 pddlstream_problem = problem_fn() print('Init:', pddlstream_problem.init) print('Goal:', pddlstream_problem.goal) info = { # Intentionally, misleading the stream 'increment': StreamInfo(p_success=0.01, overhead=1), 'decrement': StreamInfo(p_success=1, overhead=1), } if args.algorithm == 'focused': solution = solve_focused(pddlstream_problem, stream_info=info, planner='max-astar', effort_weight=1) elif args.algorithm == 'incremental': solution = solve_incremental( pddlstream_problem, #success_cost=0., max_iterations=3, max_time=5, debug=False, verbose=True) else: raise ValueError(args.algorithm) print_solution(solution)
def main(focused=False, deterministic=False, unit_costs=True): np.set_printoptions(precision=2) if deterministic: seed = 0 np.random.seed(seed) print('Seed:', get_random_seed()) problem_fn = get_tight_problem # get_tight_problem | get_blocked_problem tamp_problem = problem_fn() print(tamp_problem) stream_info = { #'test-region': StreamInfo(eager=True, p_success=0), # bound_fn is None #'plan-motion': StreamInfo(p_success=1), # bound_fn is None #'trajcollision': StreamInfo(p_success=1), # bound_fn is None #'cfree': StreamInfo(eager=True), } pddlstream_problem = pddlstream_from_tamp(tamp_problem) pr = cProfile.Profile() pr.enable() if focused: solution = solve_focused(pddlstream_problem, stream_info=stream_info, max_time=10, max_cost=INF, debug=False, effort_weight=None, unit_costs=unit_costs, postprocess=False, visualize=False) else: solution = solve_incremental(pddlstream_problem, layers=1, unit_costs=unit_costs, verbose=False) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) if plan is None: return colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS)) viewer = ContinuousTMPViewer(tamp_problem.regions, title='Continuous TAMP') state = tamp_problem.initial print() print(state) draw_state(viewer, state, colors) for i, (action, args) in enumerate(plan): user_input('Continue?') print(i, action, args) s2 = args[-1] state = TAMPState( s2[R], s2[H], {b: s2[b] for b in state.block_poses if s2[b] is not None}) print(state) draw_state(viewer, state, colors) user_input('Finish?')
def solve_pddlstream(focused=True, planner='max-astar', unit_costs=False): problem = get_problem() print(problem.constant_map) print(problem.init) print(problem.goal) stream_info = { 't-ge': StreamInfo(eager=True), 'withdraw': StreamInfo(opt_gen_fn=PartialInputs(unique=True)), } with Profiler(field='cumtime'): if focused: solution = solve_focused(problem, stream_info=stream_info, planner=planner, unit_costs=unit_costs, initial_complexity=3, clean=False, debug=True, verbose=True) else: solution = solve_incremental(problem, planner=planner, unit_costs=unit_costs, clean=False, debug=False, verbose=True) print_solution(solution) plan, cost, certificate = solution print('Certificate:', certificate.preimage_facts)
def main(): parser = argparse.ArgumentParser() parser.add_argument('-a', '--attachments', action='store_true') parser.add_argument('-o', '--optimal', action='store_true', help='Runs in an anytime mode') tamp_problem, args = initialize(parser) stream_info = { 't-region': StreamInfo(eager=False, p_success=0), 'distance': FunctionInfo(opt_fn=lambda q1, q2: MOVE_COST), } pddlstream_problem = pddlstream_from_tamp(tamp_problem) dump_pddlstream(pddlstream_problem) success_cost = 0 if args.optimal else INF planner = 'max-astar' #planner = 'ff-wastar1' with Profiler(): if args.attachments: solution = solve_incremental(pddlstream_problem, planner='ff-wastar1', max_time=args.max_time, verbose=True) else: solution = solve_focused(pddlstream_problem, stream_info=stream_info, planner=planner, max_planner_time=10, debug=False, max_time=args.max_time, max_iterations=INF, verbose=True, unit_costs=args.unit, success_cost=success_cost, unit_efforts=False, effort_weight=0, max_skeletons=None, bind=True, visualize=args.visualize) print_solution(solution) plan, cost, evaluations = solution step_plan(tamp_problem, plan)
def main(planner='max-astar', unit_costs=True, defer=False): parser = argparse.ArgumentParser() parser.add_argument('-a', '--algorithm', default='focused', help='Specifies the algorithm') args = parser.parse_args() print('Arguments:', args) pddlstream_problem = pddlstream_from_belief() _, _, _, _, init, goal = pddlstream_problem print('Init:', sorted(init, key=lambda f: f[0])) print('Goal:', goal) stream_info = { 'motion': StreamInfo(defer_fn=defer_shared if defer else never_defer), } replan_actions = set() #replan_actions = {'phone'} pr = cProfile.Profile() pr.enable() if args.algorithm == 'focused': solution = solve_focused(pddlstream_problem, stream_info=stream_info, replan_actions=replan_actions, planner=planner, unit_costs=unit_costs) elif args.algorithm == 'incremental': solution = solve_incremental(pddlstream_problem, planner=planner, unit_costs=unit_costs) else: raise NotImplementedError(args.algorithm) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(5) print_solution(solution)
def solve_pddlstream_satisfaction(stream_pddl, stream_map, init, constraints, incremental=False, **kwargs): # TODO: prune set of streams based on constraints domain, goal = planning_from_satisfaction(init, constraints) constant_map = {} problem = PDDLProblem(domain, constant_map, stream_pddl, stream_map, init, goal) if incremental: plan, cost, facts = solve_incremental(problem, **kwargs) else: plan, cost, facts = solve_focused(problem, **kwargs) if plan is None: return None, cost, facts assert len(plan) == len(domain.actions) bindings = {} for action, (name, args) in safe_zip(domain.actions, plan): assert action.name == name for param, arg in safe_zip(action.parameters, args): name = param.name assert bindings.get(name, arg) is arg bindings[name] = arg return bindings, cost, facts
def main(): parser = argparse.ArgumentParser() #parser.add_argument('-p', '--problem', default='blocked', help='The name of the problem to solve') parser.add_argument('-a', '--algorithm', default='focused', help='Specifies the algorithm') args = parser.parse_args() print('Arguments:', args) problem_fn = get_problem1 # get_problem1 | get_problem2 pddlstream_problem = problem_fn() print('Init:', pddlstream_problem.init) print('Goal:', pddlstream_problem.goal) info = { # Intentionally, misleading the stream 'increment': StreamInfo(p_success=0.01, overhead=1), 'decrement': StreamInfo(p_success=1, overhead=1), } if args.algorithm == 'focused': solution = solve_focused(pddlstream_problem, stream_info=info, planner='max-astar', effort_weight=1) elif args.algorithm == 'incremental': solution = solve_incremental(pddlstream_problem) else: raise ValueError(args.algorithm) print_solution(solution)
def solve_pddlstream(focused=False): pddlstream_problem = get_problem() if focused: solution = solve_focused(pddlstream_problem, unit_costs=True) else: solution = solve_incremental(pddlstream_problem, unit_costs=True, planner='cerberus', debug=False) print_solution(solution)
def main(): uniform_rooms = UniformDist(['room0', OTHER]) #uniform_tables = UniformDist(['table0', 'table1']) #uniform_tables = UniformDist(['table0', OTHER]) uniform_tables = UniformDist(['table0', 'table1', OTHER]) #initial_belief = get_room_belief(uniform_rooms, uniform_tables, 1.0) initial_belief = get_room_belief(uniform_rooms, uniform_tables, 0.2) #initial_belief = get_table_belief(uniform_tables, 1.0) #initial_belief = get_table_belief(uniform_tables, 0.2) #initial_belief = get_item_belief() pddlstream_problem = pddlstream_from_belief(initial_belief) _, _, _, _, init, goal = pddlstream_problem print(sorted(init)) print(goal) pr = cProfile.Profile() pr.enable() planner = 'max-astar' #solution = solve_incremental(pddlstream_problem, planner=planner, unit_costs=False) solution = solve_focused(pddlstream_problem, planner=planner, unit_costs=False) print_solution(solution) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10)
def main(deterministic=False, observable=False, collisions=True, focused=True, factor=True): # TODO: global search over the state belief_problem = get_belief_problem(deterministic, observable) pddlstream_problem = to_pddlstream(belief_problem, collisions) pr = cProfile.Profile() pr.enable() planner = 'ff-wastar1' if focused: stream_info = { 'GE': StreamInfo(from_test(ge_fn), eager=False), 'prob-after-move': StreamInfo(from_fn(get_opt_move_fn(factor=factor))), 'MoveCost': FunctionInfo(move_cost_fn), 'prob-after-look': StreamInfo(from_fn(get_opt_obs_fn(factor=factor))), 'LookCost': FunctionInfo(get_look_cost_fn(p_look_fp=0, p_look_fn=0)), } solution = solve_focused(pddlstream_problem, stream_info=stream_info, planner=planner, debug=False, max_cost=0, unit_costs=False, max_time=30) else: solution = solve_incremental(pddlstream_problem, planner=planner, debug=True, max_cost=MAX_COST, unit_costs=False, max_time=30) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) print_solution(solution) plan, cost, init = solution print('Real cost:', float(cost)/SCALE_COST)
def test_placement_on_platform(agent): """ Tests setting up a PDDL problem with each block on the platform. Intended to ensure the given domain/stream can complete the experiment setup. """ init = agent._get_initial_pddl_state() # Create the problem for one block on the platform. tform = numpy.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.1], [0., 0., 0., 1.]]) init += [('RelPose', agent.pddl_blocks[1], agent.platform, tform)] goal = ('On', agent.pddl_blocks[1], agent.platform) print('Blocks:', [b.get_name() for b in agent.pddl_blocks]) print('Init:', init) print('Goal:', goal) agent.robot.arm.hand.Open() saved_world = pb_robot.utils.WorldSaver() pddlstream_problem = tuple([*agent.pddl_info, init, goal]) plan, cost, evaluations = solve_focused(pddlstream_problem, success_cost=numpy.inf) if plan is None: print("No plan found") else: saved_world.restore() input("Execute?") ExecuteActions(plan)
def plan_trajectories(task, context, collisions=True, max_time=180): stream_info = { 'TrajPoseCollision': FunctionInfo(p_success=1e-3, eager=False), 'TrajConfCollision': FunctionInfo(p_success=1e-3, eager=False), } problem = get_pddlstream_problem(task, context, collisions=collisions) pr = cProfile.Profile() pr.enable() solution = solve_focused(problem, stream_info=stream_info, planner='ff-wastar2', success_cost=INF, max_time=max_time, debug=False, unit_efforts=True, effort_weight=1, search_sample_ratio=0) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(5) print_solution(solution) plan, cost, evaluations = solution if plan is None: print('Unable to find a solution in under {} seconds'.format(max_time)) return None return postprocess_plan(task.mbp, task.gripper, plan)
def main(viewer=False, display=True, simulate=False, teleport=False): # TODO: fix argparse & FastDownward #parser = argparse.ArgumentParser() # Automatically includes help #parser.add_argument('-viewer', action='store_true', help='enable viewer.') #parser.add_argument('-display', action='store_true', help='enable viewer.') #args = parser.parse_args() connect(use_gui=viewer) problem_fn = holding_problem # holding_problem | stacking_problem | cleaning_problem | cooking_problem # cleaning_button_problem | cooking_button_problem problem = problem_fn() state_id = save_state() #saved_world = WorldSaver() dump_world() pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport) _, _, _, stream_map, init, goal = pddlstream_problem synthesizers = [ #StreamSynthesizer('safe-base-motion', {'plan-base-motion': 1, # 'TrajPoseCollision': 0, 'TrajGraspCollision': 0, 'TrajArmCollision': 0}, # from_fn(get_base_motion_synth(problem, teleport))), ] print('Init:', init) print('Goal:', goal) print('Streams:', stream_map.keys()) print('Synthesizers:', synthesizers) pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, synthesizers=synthesizers, success_cost=INF) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) if plan is None: return if (not display) or (plan is None): disconnect() return if viewer: restore_state(state_id) else: disconnect() connect(use_gui=True) problem = problem_fn() # TODO: way of doing this without reloading? user_input('Execute?') commands = post_process(problem, plan) if simulate: enable_gravity() control_commands(commands) else: step_commands(commands, time_step=0.01) user_input('Finish?') disconnect()
def solve_pddlstream(focused=True): pddlstream_problem = get_problem() if focused: solution = solve_focused(pddlstream_problem, unit_costs=True) else: #solution = solve_exhaustive(pddlstream_problem, unit_costs=True) solution = solve_incremental(pddlstream_problem, unit_costs=True) print_solution(solution)
def main(display=True, teleport=False): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') parser.add_argument('-simulate', action='store_true', help='enable viewer.') args = parser.parse_args() connect(use_gui=args.viewer) robot, names, movable = load_world() saved_world = WorldSaver() #dump_world() pddlstream_problem = pddlstream_from_problem(robot, movable=movable, teleport=teleport) _, _, _, stream_map, init, goal = pddlstream_problem synthesizers = [ StreamSynthesizer('safe-free-motion', {'plan-free-motion': 1, 'trajcollision': 0}, from_fn(get_free_motion_synth(robot, movable, teleport))), StreamSynthesizer('safe-holding-motion', {'plan-holding-motion': 1, 'trajcollision': 0}, from_fn(get_holding_motion_synth(robot, movable, teleport))), ] if USE_SYNTHESIZERS else [] print('Init:', init) print('Goal:', goal) print('Streams:', stream_map.keys()) print('Synthesizers:', stream_map.keys()) print(names) pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, synthesizers=synthesizers, success_cost=INF) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) if plan is None: return if (not display) or (plan is None): disconnect() return if not args.viewer: # TODO: how to reenable the viewer disconnect() connect(use_gui=True) load_world() else: saved_world.restore() command = postprocess_plan(plan) if args.simulate: user_input('Simulate?') command.control() else: user_input('Execute?') #command.step() command.refine(num_steps=10).execute(time_step=0.001) #wait_for_interrupt() user_input('Finish?') disconnect()
def plan_sequence(robot, obstacles, node_points, element_bodies, ground_nodes, trajectories=[]): if trajectories is None: return None pddlstream_fn = get_pddlstream2 # get_pddlstream | get_pddlstream2 # TODO: Iterated search # http://www.fast-downward.org/Doc/Heuristic#h.5Em_heuristic # randomize_successors=True pr = cProfile.Profile() pr.enable() pddlstream_problem = pddlstream_fn(robot, obstacles, node_points, element_bodies, ground_nodes, trajectories=trajectories) #solution = solve_exhaustive(pddlstream_problem, planner='goal-lazy', max_time=300, debug=True) #solution = solve_incremental(pddlstream_problem, planner='add-random-lazy', max_time=600, # max_planner_time=300, debug=True) stream_info = { #'sample-print': StreamInfo(PartialInputs(unique=True)), } planner = 'ff-ehc' #planner = 'ff-wastar1000' # Branching factor becomes large. Rely on preferred. Preferred should also be cheaper #planner = 'ff-eager-wastar1000' #planner = 'ff-wastar5' solution = solve_focused( pddlstream_problem, stream_info=stream_info, max_time=600, effort_weight=1, unit_efforts=True, use_skeleton=False, #unit_costs=True, planner=planner, max_planner_time=15, debug=True) # solve_exhaustive | solve_incremental # Reachability heuristics good for detecting dead-ends # Infeasibility from the start means disconnected or collision # Random restart based strategy here print_solution(solution) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) plan, _, _ = solution if plan is None: return None if pddlstream_fn == get_pddlstream: return [t for _, (n1, e, t, n2) in plan] elif pddlstream_fn == get_pddlstream2: return [t for _, (n1, e, t) in reversed(plan)] else: raise ValueError(pddlstream_fn)
def main(): # TODO: maybe load problems as a domain explicitly pddlstream_problem = get_pddlstream_problem() stream_info = { #'test-feasible': StreamInfo(negate=True), } solution = solve_focused(pddlstream_problem, stream_info=stream_info) #solution = solve_incremental(pddlstream_problem) # Should throw an error print_solution(solution)
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 solve_pddlstream(focused=True): problem_fn = get_problem1 # get_problem1 | get_problem2 pddlstream_problem = problem_fn() print('Init:', pddlstream_problem.init) print('Goal:', pddlstream_problem.goal) if focused: solution = solve_focused(pddlstream_problem, unit_costs=True) else: solution = solve_incremental(pddlstream_problem, unit_costs=True) print_solution(solution)
def solve_pddlstream_satisfaction(stream_pddl, stream_map, init, constraints, incremental=False, **kwargs): # TODO: prune set of streams based on constraints # TODO: investigate constraint satisfaction techniques for search instead # TODO: optimistic objects based on free parameters that prevent cycles # TODO: disallow creation of new parameters / certifying new facts problem = pddl_from_csp(stream_pddl, stream_map, init, constraints) if incremental: plan, cost, facts = solve_incremental(problem, **kwargs) else: plan, cost, facts = solve_focused(problem, **kwargs) bindings = bindings_from_plan(problem, plan) return bindings, cost, facts
def main(replan=True, defer=True): pddlstream_problem = get_pddlstream() stream_info = { #'test-pose': StreamInfo(eager=True, p_success=0), 'motion': StreamInfo(defer_fn=defer_unique if defer else never_defer), } replan_actions = {'pick'} if replan else set() with Profiler(): solution = solve_focused(pddlstream_problem, stream_info=stream_info, replan_actions=replan_actions) print_solution(solution)
def plan_sequence(robot, obstacles, node_points, element_bodies, ground_nodes, trajectories=[], collisions=True, debug=False, max_time=30): if trajectories is None: return None # TODO: iterated search using random restarts # TODO: most of the time seems to be spent extracting the stream plan # TODO: NEGATIVE_SUFFIX to make axioms easier pr = cProfile.Profile() pr.enable() pddlstream_problem = get_pddlstream(robot, obstacles, node_points, element_bodies, ground_nodes, trajectories=trajectories, collisions=collisions) #solution = solve_exhaustive(pddlstream_problem, planner='goal-lazy', max_time=300, debug=True) #solution = solve_incremental(pddlstream_problem, planner='add-random-lazy', max_time=600, # max_planner_time=300, debug=True) stream_info = { 'sample-print': StreamInfo(PartialInputs(unique=True)), } #planner = 'ff-ehc' planner = 'ff-lazy-tiebreak' # Branching factor becomes large. Rely on preferred. Preferred should also be cheaper solution = solve_focused(pddlstream_problem, stream_info=stream_info, max_time=max_time, effort_weight=1, unit_efforts=True, use_skeleton=False, unit_costs=True, planner=planner, max_planner_time=15, debug=debug, reorder=False) # Reachability heuristics good for detecting dead-ends # Infeasibility from the start means disconnected or collision print_solution(solution) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) plan, _, _ = solution if plan is None: return None return [t for _, (n1, e, t) in reversed(plan)]
def main(): parser = argparse.ArgumentParser() # choreo_brick_demo | choreo_eth-trees_demo parser.add_argument('-p', '--problem', default='choreo_brick_demo', help='The name of the problem to solve') parser.add_argument('-c', '--cfree', action='store_true', help='Disables collisions with obstacles') parser.add_argument('-t', '--teleport', action='store_true', help='Teleports instead of computing motions') parser.add_argument('-v', '--viewer', action='store_true', help='Enables the viewer during planning (slow!)') args = parser.parse_args() print('Arguments:', args) connect(use_gui=args.viewer) robot, brick_from_index, obstacle_from_name = load_pick_and_place( args.problem) np.set_printoptions(precision=2) pr = cProfile.Profile() pr.enable() with WorldSaver(): pddlstream_problem = get_pddlstream(robot, brick_from_index, obstacle_from_name, collisions=not args.cfree, teleport=args.teleport) with LockRenderer(): solution = solve_focused(pddlstream_problem, planner='ff-wastar1', max_time=30) pr.disable() pstats.Stats(pr).sort_stats('cumtime').print_stats(10) print_solution(solution) plan, _, _ = solution if plan is None: return if not has_gui(): connect(use_gui=True) _ = load_pick_and_place(args.problem) step_plan(plan, time_step=(np.inf if args.teleport else 0.01))
def plan_trajectories(task, context, collisions=True, max_time=180): stream_info = { 'TrajPoseCollision': FunctionInfo(p_success=1e-3), 'TrajConfCollision': FunctionInfo(p_success=1e-3), } problem = get_pddlstream_problem(task, context, collisions=collisions) solution = solve_focused(problem, stream_info=stream_info, planner='ff-wastar2', max_time=max_time, search_sampling_ratio=0) print_solution(solution) plan, cost, evaluations = solution if plan is None: print('Unable to find a solution in under {} seconds'.format(max_time)) return None return postprocess_plan(task.mbp, task.gripper, plan)
def main(focused=True, unit_costs=False): problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem tamp_problem = problem_fn() print(tamp_problem) pddlstream_problem = pddlstream_from_tamp(tamp_problem) if focused: solution = solve_focused(pddlstream_problem, unit_costs=unit_costs) else: solution = solve_incremental(pddlstream_problem, unit_costs=unit_costs) print_solution(solution) plan, cost, evaluations = solution if plan is None: return apply_plan(tamp_problem, plan)
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 main(focused=True): # TODO: maybe load problems as a domain explicitly pddlstream_problem = pddlstream_from_belief() _, _, _, _, init, goal = pddlstream_problem print('Init:', sorted(init, key=lambda f: f[0])) print('Goal:', goal) pr = cProfile.Profile() pr.enable() if focused: solution = solve_focused(pddlstream_problem, unit_costs=False) else: #solution = solve_exhaustive(pddlstream_problem, unit_costs=False) solution = solve_incremental(pddlstream_problem, unit_costs=False) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(5) print_solution(solution)
def find_plan(self, ros_goal): """ Main PDDLStream planning function (DEPRECATED)""" print("Planning...") self.plan() self.robot.arm.hand.Open() # Get the initial conditions and goal specification init, goal, fixed_objs = self.unpack_goal(ros_goal) print_planning_problem(init, goal, fixed_objs) saved_world = pb_robot.utils.WorldSaver() # Get PDDLStream planning information pddl_info = get_pddlstream_info(self.robot, self.pddl_blocks, add_slanted_grasps=False, approach_frame='global') # Run PDDLStream focused solver start = time.time() pddlstream_problem = tuple([*pddl_info, init, goal]) plan, _, _ = solve_focused(pddlstream_problem, success_cost=numpy.inf, max_skeletons=2, max_failures=3, search_sample_ratio=1., max_time=INF) duration = time.time() - start saved_world.restore() print('Planning Complete: Time %f seconds' % duration) print(f"\nFINAL PLAN\n{plan}\n") # Package and return the result result = TaskPlanResult() result.success = (plan is not None) result.plan = task_plan_to_ros(plan) # print(result) self.planning_service.set_succeeded(result, text="Planning complete") # Update the planning domain if result.success: self.plan() ExecuteActions(plan, real=False, pause=False, wait=False, prompt=False)
def main(max_time = 180): robot = UR5() robot.open_gripper() robot.close_gripper() robot.open_gripper() robot.go_to_pose_goal_demo() exit() problem = get_problem(robot) solution = solve_focused(problem, planner='ff-wastar2', success_cost=INF, max_time=max_time, debug=False, unit_efforts=True, effort_weight=1, search_sample_ratio=0) # print_solution(solution) plan, cost, evaluations = solution if plan is None: print('Unable to find a solution in under {} seconds'.format(max_time)) return None interpret_plan(robot, plan) return True