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(): parser = create_parser() args = parser.parse_args() print('Arguments:', args) initial_poses = { ROBOT: (0., 15., 0.), CUP: (7.5, 0., 0.), 'sugar_cup': (-10., 0., 0.), 'cream_cup': (15., 0, 0), 'spoon': (0.5, 0.5, 0), 'stirrer': (20, 0.5, 0), COASTER: (-20., 0, 0), } problem = create_problem(initial_poses) with Profiler(field='tottime'): #solution = solve_serialized(problem, planner='ff-eager', unit_costs=args.unit, # unit_efforts=True, effort_weight=1, debug=False) # max_planner_time=5, solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit, planner='ff-eager', unit_efforts=True, effort_weight=1, debug=False) # max_planner_time=5, print_solution(solution)
def main(): parser = create_parser() args = parser.parse_args() print('Arguments:', args) 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('Init:', sorted(init)) print('Goal:', goal) planner = 'max-astar' with Profiler(field='tottime', num=10): solution = solve(pddlstream_problem, algorithm=args.algorithm, unit_costs=args.unit, planner=planner) print_solution(solution)
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(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(planner='max-astar'): parser = create_parser() args = parser.parse_args() print('Arguments:', args) problem = get_problem(optimize=True) 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'): solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit, stream_info=stream_info, planner=planner, initial_complexity=3, clean=False, debug=True, verbose=True) print_solution(solution) plan, cost, certificate = solution print('Certificate:', certificate.preimage_facts)
def main(deterministic=False, observable=False, collisions=True, factor=True): parser = create_parser() args = parser.parse_args() print('Arguments:', args) # TODO: AssertionError: degenerate distribution DDist{l0: 0.00000} # TODO: global search over the state belief_problem = get_belief_problem(deterministic, observable) pddlstream_problem = to_pddlstream(belief_problem, collisions) print('Cost scale:', get_cost_scale()) 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)), } planner = 'ff-wastar1' success_cost = 0 # 0 | MAX_COST with Profiler(field='tottime', num=10): solution = solve(pddlstream_problem, algorithm=args.algorithm, unit_costs=args.unit, stream_info=stream_info, planner=planner, debug=False, success_cost=success_cost, max_time=30) print_solution(solution)
def main(planner='max-astar', defer=False): parser = create_parser() args = parser.parse_args() print('Arguments:', args) problem = pddlstream_from_belief() _, _, _, _, init, goal = 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'} with Profiler(field='tottime', num=5): solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit, planner=planner, stream_info=stream_info, replan_actions=replan_actions) print_solution(solution)
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 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(): 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(): 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 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(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 = tight # 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, success_cost=INF, debug=False, effort_weight=None, unit_costs=unit_costs, visualize=False) else: solution = solve_incremental(pddlstream_problem, complexity_step=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 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 solve_pddlstream(debug=False): parser = create_parser() args = parser.parse_args() print('Arguments:', args) problem = get_problem() planner = 'lmcut-astar' # cerberus solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit, planner=planner, debug=debug) 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 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 solve_pddlstream(): parser = create_parser() args = parser.parse_args() print('Arguments:', args) problem = get_problem1() print('Init:', problem.init) print('Goal:', problem.goal) solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit) print_solution(solution)
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 main(): parser = create_parser() args = parser.parse_args() print('Arguments:', args) # TODO: maybe load problems as a domain explicitly problem = get_pddlstream_problem() #if args.algorithm not in FOCUSED_ALGORITHMS: # raise RuntimeError('The {} algorithm does not support fluent streams'.format(args.algorithm)) solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit) print_solution(solution)
def plan_sequence_test(node_points, elements, ground_nodes): pr = cProfile.Profile() pr.enable() pddlstream_problem = get_pddlstream_test(node_points, elements, ground_nodes) #solution = solve_focused(pddlstream_problem, planner='goal-lazy', max_time=10, debug=False) solution = solve_exhaustive(pddlstream_problem, planner='goal-lazy', max_time=10, debug=False) print_solution(solution) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) plan, _, _ = solution return plan
def main(): parser = argparse.ArgumentParser() parser.add_argument('-p', '--problem', default='mirror', help='The name of the problem to solve') parser.add_argument('-a', '--algorithm', default='incremental', help='Specifies the algorithm') parser.add_argument('-c', '--cfree', action='store_true', help='Disables collisions') parser.add_argument('-d', '--deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-g', '--gurobi', action='store_true', help='Uses gurobi') parser.add_argument('-n', '--number', default=1, type=int, help='The number of blocks') parser.add_argument('-o', '--optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-s', '--skeleton', action='store_true', help='Enforces skeleton plan constraints') parser.add_argument('-t', '--max_time', default=30, type=int, help='The max time') parser.add_argument('-u', '--unit', action='store_true', help='Uses unit costs') parser.add_argument('-v', '--visualize', action='store_true', help='Visualizes graphs') args = parser.parse_args() print('Arguments:', args) np.set_printoptions(precision=2) if args.deterministic: random.seed(seed=0) np.random.seed(seed=0) print('Random seed:', get_random_seed()) problem_from_name = {fn.__name__: fn for fn in PROBLEMS} if args.problem not in problem_from_name: raise ValueError(args.problem) print('Problem:', args.problem) problem_fn = problem_from_name[args.problem] tamp_problem = problem_fn(args.number) print(tamp_problem) pddlstream_problem = pddlstream_from_tamp(tamp_problem, collisions=not args.cfree, use_stream=not args.gurobi, use_optimizer=args.gurobi) print('Constants:', str_from_object(pddlstream_problem.constant_map)) print('Initial:', sorted_str_from_list(pddlstream_problem.init)) print('Goal:', str_from_object(pddlstream_problem.goal)) success_cost = 0 if args.optimal else INF planner = 'max-astar' #planner = 'ff-wastar1' with Profiler(field='cumtime', num=20): if args.algorithm == 'incremental': solution = solve_incremental(pddlstream_problem, complexity_step=1, planner=planner, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=False) else: raise ValueError(args.algorithm) print_solution(solution) plan, cost, evaluations = solution if plan is not None: display_plan(tamp_problem, plan)
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(max_time=20): """ Creates and solves the 2D motion planning problem. """ parser = create_parser() args = parser.parse_args() print('Arguments:', args) obstacles = [create_box((.5, .5), (.2, .2))] regions = { 'env': create_box((.5, .5), (1, 1)), 'green': create_box((.8, .8), (.4, .4)), } goal = 'green' if goal not in regions: goal = ARRAY([1, 1]) max_distance = 0.25 # 0.2 | 0.25 | 0.5 | 1.0 problem, samples, roadmap = create_problem(goal, obstacles, regions, max_distance=max_distance) print('Initial:', str_from_object(problem.init)) print('Goal:', str_from_object(problem.goal)) constraints = PlanConstraints(max_cost=1.25) # max_cost=INF) with Profiler(field='tottime', num=10): solution = solve_incremental(problem, constraints=constraints, unit_costs=args.unit, success_cost=0, max_time=max_time, verbose=False) print_solution(solution) plan, cost, evaluations = solution #viewer = draw_environment(obstacles, regions) #for sample in samples: # viewer.draw_point(sample) #user_input('Continue?') # TODO: use the same viewer here draw_roadmap(roadmap, obstacles, regions) # TODO: do this in realtime user_input('Continue?') if plan is None: return segments = [args for name, args in plan] draw_solution(segments, obstacles, regions) user_input('Finish?')
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 main(): parser = create_parser() args = parser.parse_args() print('Arguments:', args) streams, orders, info = create_problem2() # create_problem1 | create_problem2 opt_solution = opt_from_graph(streams, orders, info) print(SEPARATOR) solution = solve_skeleton(opt_solutions=[opt_solution]) print(SEPARATOR) print(solution) # TODO: print the stream plan print_solution(solution)