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 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(display=True, teleport=False, partial=False, defer=False): parser = argparse.ArgumentParser() parser.add_argument('-simulate', action='store_true', help='Simulates the system') 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'), defer_fn=defer_shared if defer else never_defer), 'MoveCost': FunctionInfo(opt_move_cost_fn), } if partial else { 'sample-pose': StreamInfo(from_fn(opt_pose_fn)), 'inverse-kinematics': StreamInfo(from_fn(opt_ik_fn)), 'plan-base-motion': StreamInfo(from_fn(opt_motion_fn)), 'MoveCost': FunctionInfo(opt_move_cost_fn), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) print('Streams:', stream_map.keys()) pr = cProfile.Profile() pr.enable() with LockRenderer(): #solution = solve_incremental(pddlstream_problem, debug=True) solution = solve_focused(pddlstream_problem, stream_info=stream_info, success_cost=INF, debug=False) 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 with LockRenderer(): 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 args.simulate: control_commands(commands) else: apply_commands(State(), commands, time_step=0.01) user_input('Finish?') disconnect()