def display_plan(problem, state, plan, time_step=0.01, sec_per_step=0.005): duration = compute_duration(plan) real_time = None if sec_per_step is None else (duration * sec_per_step / time_step) print('Duration: {} | Step size: {} | Real time: {}'.format(duration, time_step, real_time)) colors = {robot: COLOR_FROM_NAME[robot] for robot in problem.robots} for i, t in enumerate(inclusive_range(0, duration, time_step)): print('Step={} | t={}'.format(i, t)) for action in plan: name, args, start, duration = action if not (action.start <= t <= get_end(action)): continue if name == 'move': robot, conf1, traj, conf2 = args traj = [conf1.values, conf2.values] fraction = (t - action.start) / action.duration conf = get_value_at_time(traj, fraction) body = problem.get_body(robot) color = COLOR_FROM_NAME[robot] if colors[robot] != color: set_color(body, color, link_from_name(body, TOP_LINK)) colors[robot] = color set_base_conf(body, conf) elif name == 'recharge': robot, conf = args body = problem.get_body(robot) color = YELLOW if colors[robot] != color: set_color(body, color, link_from_name(body, TOP_LINK)) colors[robot] = color else: raise ValueError(name) if sec_per_step is None: user_input('Continue?') else: wait_for_duration(sec_per_step)
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, 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(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) robot, names, movable = load_world() saved_world = WorldSaver() dump_world() pddlstream_problem = pddlstream_from_problem(robot, movable=movable, teleport=teleport, movable_collisions=True) _, _, _, 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))), ] 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, max_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 paths = [] for name, args in plan: if name == 'place': paths += args[-1].reverse().body_paths elif name in ['move', 'move_free', 'move_holding', 'pick']: paths += args[-1].body_paths print(paths) command = Command(paths) if not viewer: # TODO: how to reenable the viewer disconnect() connect(use_gui=True) load_world() else: saved_world.restore() user_input('Execute?') if simulate: command.control() else: #command.step() command.refine(num_steps=10).execute(time_step=0.001) #wait_for_interrupt() 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()