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 main(display=True, simulate=False, teleport=False): parser = argparse.ArgumentParser() parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') #parser.add_argument('-display', action='store_true', help='displays the solution') args = parser.parse_args() connect(use_gui=args.viewer) problem_fn = cooking_problem # holding_problem | stacking_problem | cleaning_problem | cooking_problem # cleaning_button_problem | cooking_button_problem with HideOutput(): problem = problem_fn() state_id = save_state() #saved_world = WorldSaver() #dump_world() pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport) stream_info = { 'sample-pose': StreamInfo(PartialInputs('?r')), 'inverse-kinematics': StreamInfo(PartialInputs('?p')), 'plan-base-motion': StreamInfo(PartialInputs('?q1 ?q2')), 'MoveCost': FunctionInfo(opt_move_cost_fn), } synthesizers = [ StreamSynthesizer( 'safe-base-motion', { 'plan-base-motion': 1, 'TrajPoseCollision': 0, 'TrajGraspCollision': 0, 'TrajArmCollision': 0, }, from_fn(get_base_motion_synth(problem, teleport))), ] if USE_SYNTHESIZERS else [] _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) print('Streams:', stream_map.keys()) print('Synthesizers:', synthesizers) pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, stream_info=stream_info, 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 commands = post_process(problem, plan) if args.viewer: restore_state(state_id) else: disconnect() connect(use_gui=True) with HideOutput(): problem_fn() # TODO: way of doing this without reloading? if simulate: enable_gravity() control_commands(commands) else: step_commands(commands, time_step=0.01) user_input('Finish?') disconnect()
def main(focused=True, deterministic=True, unit_costs=False, use_synthesizers=False): np.set_printoptions(precision=2) if deterministic: seed = 0 np.random.seed(seed) print('Seed:', get_random_seed()) if use_synthesizers and not has_gurobi(): use_synthesizers = False print( 'Warning! use_synthesizers=True requires gurobipy. Setting use_synthesizers=False.' ) print('Focused: {} | Costs: {} | Synthesizers: {}'.format( focused, not unit_costs, use_synthesizers)) problem_fn = get_blocked_problem # get_tight_problem | get_blocked_problem tamp_problem = problem_fn() print(tamp_problem) action_info = { #'move': ActionInfo(terminal=True), #'pick': ActionInfo(terminal=True), #'place': ActionInfo(terminal=True), } stream_info = { 't-region': StreamInfo(eager=True, p_success=0), # bound_fn is None 't-cfree': StreamInfo(eager=False, negate=True), #'distance': FunctionInfo(opt_fn=lambda *args: 1), #'gurobi': OptimizerInfo(p_success=0), #'rrt': OptimizerInfo(p_success=0), } hierarchy = [ #ABSTRIPSLayer(pos_pre=['atconf']), #, horizon=1), ] synthesizers = [ #StreamSynthesizer('cfree-motion', {'s-motion': 1, 'trajcollision': 0}, # gen_fn=from_fn(cfree_motion_fn)), StreamSynthesizer('optimize', { 's-region': 1, 's-ik': 1, 'posecollision': 0, 't-cfree': 0, 'distance': 0 }, gen_fn=from_fn(get_optimize_fn( tamp_problem.regions))), ] if use_synthesizers else [] pddlstream_problem = pddlstream_from_tamp(tamp_problem) print('Initial:', str_from_object(pddlstream_problem.init)) print('Goal:', str_from_object(pddlstream_problem.goal)) pr = cProfile.Profile() pr.enable() if focused: solution = solve_focused( pddlstream_problem, action_info=action_info, stream_info=stream_info, planner='ff-wastar1', max_planner_time=10, synthesizers=synthesizers, verbose=True, max_time=300, max_cost=INF, debug=False, hierarchy=hierarchy, effort_weight=1, search_sampling_ratio=0, # TODO: run without to see difference unit_costs=unit_costs, postprocess=False, visualize=False) else: solution = solve_incremental(pddlstream_problem, layers=1, hierarchy=hierarchy, 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 not None: display_plan(tamp_problem, plan)
def main(use_synthesizers=False): 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') 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('-u', '--unit', action='store_true', help='Uses unit costs') parser.add_argument('-o', '--optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=20, type=int, help='The max time') args = parser.parse_args() print('Arguments:', args) print('Synthesizers: {}'.format(use_synthesizers)) np.set_printoptions(precision=2) if args.deterministic: seed = 0 np.random.seed(seed) print('Random seed:', get_random_seed()) if use_synthesizers and not has_gurobi(): use_synthesizers = False print('Warning! use_synthesizers=True requires gurobipy. Setting use_synthesizers=False.') if args.problem not in PROBLEMS: raise ValueError(args.problem) print('Problem:', args.problem) problem_fn = PROBLEMS[args.problem] tamp_problem = problem_fn() print(tamp_problem) action_info = { #'move': ActionInfo(terminal=True), #'pick': ActionInfo(terminal=True), #'place': ActionInfo(terminal=True), } stream_info = { 't-region': StreamInfo(eager=True, p_success=0), # bound_fn is None 't-cfree': StreamInfo(eager=False, negate=True), 'distance': FunctionInfo(opt_fn=lambda q1, q2: MOVE_COST), #'gurobi': OptimizerInfo(p_success=0), #'rrt': OptimizerInfo(p_success=0), } hierarchy = [ #ABSTRIPSLayer(pos_pre=['atconf']), #, horizon=1), ] synthesizers = [ #StreamSynthesizer('cfree-motion', {'s-motion': 1, 'trajcollision': 0}, # gen_fn=from_fn(cfree_motion_fn)), StreamSynthesizer('optimize', {'s-region': 1, 's-ik': 1, 'posecollision': 0, 't-cfree': 0, 'distance': 0}, gen_fn=from_fn(get_optimize_fn(tamp_problem.regions))), ] if use_synthesizers else [] skeleton = [ ('move', ['?q0', WILD, '?q1']), ('pick', ['b0', '?p0', '?q1']), ('move', ['?q1', WILD, '?q2']), ('place', ['b0', '?p1', '?q2']), ] constraints = PlanConstraints(#skeletons=None, #skeletons=[], #skeletons=[skeleton], #skeletons=[skeleton, []], #exact=False, max_cost=INF) pddlstream_problem = pddlstream_from_tamp(tamp_problem, collisions=not args.cfree) print('Initial:', str_from_object(pddlstream_problem.init)) print('Goal:', str_from_object(pddlstream_problem.goal)) pr = cProfile.Profile() pr.enable() success_cost = 0 if args.optimal else INF if args.algorithm == 'focused': solution = solve_focused(pddlstream_problem, constraints=constraints, action_info=action_info, stream_info=stream_info, synthesizers=synthesizers, planner='ff-wastar1', max_planner_time=10, hierarchy=hierarchy, 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, search_sample_ratio=0, #max_skeletons=None, visualize=False) elif args.algorithm == 'incremental': solution = solve_incremental(pddlstream_problem, constraints=constraints, complexity_step=2, hierarchy=hierarchy, 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 pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) if plan is not None: display_plan(tamp_problem, plan)