def main(display=True, teleport=False): parser = argparse.ArgumentParser() #parser.add_argument('-problem', default='rovers1', help='The name of the problem to solve') parser.add_argument('-algorithm', default='focused', help='Specifies the algorithm') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-unit', action='store_true', help='Uses unit costs') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') args = parser.parse_args() print(args) #problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} #if args.problem not in problem_fn_from_name: # raise ValueError(args.problem) #problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn(collisions=not args.cfree) saver = WorldSaver() draw_base_limits(problem.limits, color=RED) pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport) stream_info = { 'test-cfree-conf-pose': StreamInfo(negate=True, p_success=1e-2), 'test-cfree-traj-pose': StreamInfo(negate=True, p_success=1e-1), 'compute-motion': StreamInfo(eager=True, p_success=0), 'test-reachable': StreamInfo(eager=True), 'Distance': FunctionInfo(eager=True), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) success_cost = 0 if args.optimal else INF planner = 'ff-wastar1' search_sample_ratio = 0 max_planner_time = 10 pr = cProfile.Profile() pr.enable() with LockRenderer(True): if args.algorithm == 'focused': solution = solve_focused(pddlstream_problem, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, debug=False, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, unit_efforts=True, effort_weight=1, bind=True, max_skeletons=None, search_sample_ratio=search_sample_ratio) elif args.algorithm == 'incremental': solution = solve_incremental(pddlstream_problem, planner=planner, max_planner_time=max_planner_time, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True) else: raise ValueError(args.algorithm) saver.restore() print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime if plan is None: wait_for_user() return if (not display) or (plan is None): disconnect() return with LockRenderer(): commands = post_process(problem, plan, teleport=teleport) saver.restore() # Assumes bodies are ordered the same way if not args.viewer: disconnect() connect(use_gui=True) with LockRenderer(): with HideOutput(): problem_fn() # TODO: way of doing this without reloading? saver.restore() # Assumes bodies are ordered the same way wait_for_user() if args.simulate: control_commands(commands) else: time_step = None if teleport else 0.01 apply_commands(BeliefState(problem), commands, time_step=time_step) wait_for_user() disconnect()
def post_process(state, plan, replan_obs=True, replan_base=False, look_move=True): problem = state.task if plan is None: return None # TODO: refine actions robot = problem.robot commands = [] uncertain_base = False expecting_obs = False for i, (name, args) in enumerate(plan): if replan_obs and expecting_obs: break saved_world = WorldSaver() # StateSaver() if name == 'move_base': t = args[-1] # TODO: look at the trajectory (endpoint or path) to ensure fine # TODO: I should probably move base and keep looking at the path # TODO: I could keep updating the head goal as the base moves along the path if look_move: new_commands = [move_look_trajectory(t)] #new_commands = [inspect_trajectory(t), t] else: new_commands = [t] if replan_base: uncertain_base = True elif name == 'pick': if uncertain_base: break a, b, p, g, _, t = args attach = Attach(robot, a, g, b) new_commands = [t, attach, t.reverse()] elif name == 'place': if uncertain_base: break a, b, p, g, _, t = args detach = Detach(robot, a, b) new_commands = [t, detach, t.reverse()] elif name == 'scan': o, p, bq, hq, ht = args ht0 = plan_head_traj(robot, hq.values) new_commands = [ht0] if o in problem.rooms: attach, detach = get_cone_commands(robot) new_commands += [attach, ht, ScanRoom(robot, o), detach] else: new_commands += [ht, Scan(robot, o)] #with BodySaver(robot): # for hq2 in ht.path: # st = plan_head_traj(robot, hq2.values) # new_commands += [st, Scan(robot, o)] # hq2.step() # TODO: return to start conf? elif name == 'localize': r, _, o, _ = args new_commands = [Detect(robot, r, o)] expecting_obs = True elif name == 'register': o, p, bq, hq, ht = args ht0 = plan_head_traj(robot, hq.values) register = Register(robot, o) new_commands = [ht0, register] expecting_obs = True else: raise ValueError(name) saved_world.restore() for command in new_commands: if isinstance(command, Trajectory) and command.path: command.path[-1].step() commands += new_commands return commands
def main(display=True, teleport=False): parser = argparse.ArgumentParser() parser.add_argument('-algorithm', default='incremental', help='Specifies the algorithm') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=5 * 60, type=int, help='The max time') parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') args = parser.parse_args() print(args) #problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} #if args.problem not in problem_fn_from_name: # raise ValueError(args.problem) #problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn(collisions=not args.cfree) saver = WorldSaver() draw_base_limits(problem.limits, color=RED) pddlstream, edges = pddlstream_from_problem(problem, teleport=teleport) _, constant_map, _, stream_map, init, goal = pddlstream print('Constants:', constant_map) print('Init:', init) print('Goal:', goal) success_cost = 0 if args.optimal else INF max_planner_time = 10 stream_info = { 'compute-motion': StreamInfo(eager=True, p_success=0), 'ConfConfCollision': FunctionInfo(p_success=1, overhead=0.1), 'TrajConfCollision': FunctionInfo(p_success=1, overhead=1), 'TrajTrajCollision': FunctionInfo(p_success=1, overhead=10), 'TrajDistance': FunctionInfo( eager=True ), # Need to eagerly evaluate otherwise 0 duration (failure) } pr = cProfile.Profile() pr.enable() with LockRenderer(False): if args.algorithm == 'incremental': solution = solve_incremental(pddlstream, max_planner_time=max_planner_time, success_cost=success_cost, max_time=args.max_time, start_complexity=INF, verbose=True, debug=True) elif args.algorithm == 'focused': solution = solve_focused(pddlstream, stream_info=stream_info, max_planner_time=max_planner_time, success_cost=success_cost, max_time=args.max_time, max_skeletons=None, bind=True, max_failures=INF, verbose=True, debug=True) else: raise ValueError(args.algorithm) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime if plan is None: wait_for_user() return if (not display) or (plan is None): disconnect() return if not args.viewer: disconnect() connect(use_gui=True) with LockRenderer(): with HideOutput(): problem_fn() # TODO: way of doing this without reloading? saver.restore() # Assumes bodies are ordered the same way draw_edges(edges) state = BeliefState(problem) wait_for_user() #time_step = None if teleport else 0.01 #with VideoSaver('video.mp4'): display_plan(problem, state, plan) wait_for_user() disconnect()
def main(): parser = create_parser() parser.add_argument('-problem', default='rovers1', help='The name of the problem to solve') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-enable', action='store_true', help='Enables rendering during planning') parser.add_argument('-teleport', action='store_true', help='Teleports between configurations') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan') args = parser.parse_args() print('Arguments:', args) problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} if args.problem not in problem_fn_from_name: raise ValueError(args.problem) problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): rovers_problem = problem_fn() saver = WorldSaver() draw_base_limits(rovers_problem.limits, color=RED) pddlstream_problem = pddlstream_from_problem(rovers_problem, collisions=not args.cfree, teleport=args.teleport, holonomic=False, reversible=True, use_aabb=True) stream_info = { 'test-cfree-ray-conf': StreamInfo(), 'test-reachable': StreamInfo(p_success=1e-1), 'obj-inv-visible': StreamInfo(), 'com-inv-visible': StreamInfo(), 'sample-above': StreamInfo(), 'sample-motion': StreamInfo(overhead=10), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) #print('Streams:', stream_map.keys()) success_cost = 0 if args.optimal else INF planner = 'ff-wastar3' search_sample_ratio = 2 max_planner_time = 10 # TODO: need to accelerate samples here because of the failed test reachable with Profiler(field='tottime', num=25): with LockRenderer(lock=not args.enable): # TODO: option to only consider costs during local optimization solution = solve(pddlstream_problem, algorithm=args.algorithm, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, debug=False, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, unit_efforts=True, effort_weight=1, search_sample_ratio=search_sample_ratio) for body in get_bodies(): if body not in saver.bodies: remove_body(body) saver.restore() print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return # Maybe OpenRAVE didn't actually sample any joints... # http://openrave.org/docs/0.8.2/openravepy/examples.tutorial_iksolutions/ with LockRenderer(): commands = post_process(rovers_problem, plan) saver.restore() wait_for_user('Begin?') if args.simulate: control_commands(commands) else: time_step = None if args.teleport else 0.01 apply_commands(BeliefState(rovers_problem), commands, time_step) wait_for_user('Finish?') disconnect()
def main(display=True, teleport=False): parser = argparse.ArgumentParser() parser.add_argument('-problem', default='rovers1', help='The name of the problem to solve') parser.add_argument('-algorithm', default='focused', help='Specifies the algorithm') parser.add_argument('-cfree', action='store_true', help='Disables collisions') parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler') parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode') parser.add_argument('-t', '--max_time', default=120, type=int, help='The max time') parser.add_argument('-unit', action='store_true', help='Uses unit costs') parser.add_argument('-simulate', action='store_true', help='Simulates the system') parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') args = parser.parse_args() print(args) problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS} if args.problem not in problem_fn_from_name: raise ValueError(args.problem) problem_fn = problem_fn_from_name[args.problem] connect(use_gui=args.viewer) with HideOutput(): problem = problem_fn() saver = WorldSaver() draw_base_limits(problem.limits, color=(1, 0, 0)) pddlstream_problem = pddlstream_from_problem(problem, collisions=not args.cfree, teleport=teleport) stream_info = { 'test-cfree-ray-conf': StreamInfo(negate=True), 'test-reachable': StreamInfo(p_success=1e-1), 'obj-inv-visible': StreamInfo(), 'com-inv-visible': StreamInfo(), 'sample-above': StreamInfo(), 'sample-motion': StreamInfo(overhead=10), } _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) #print('Streams:', stream_map.keys()) success_cost = 0 if args.optimal else INF planner = 'ff-wastar3' search_sample_ratio = 2 max_planner_time = 10 # TODO: need to accelerate samples here because of the failed test reachable pr = cProfile.Profile() pr.enable() with LockRenderer(False): if args.algorithm == 'focused': # TODO: option to only consider costs during local optimization solution = solve_focused( pddlstream_problem, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, debug=False, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, unit_efforts=True, effort_weight=1, #bind=True, max_skeletons=None, search_sample_ratio=search_sample_ratio) elif args.algorithm == 'incremental': solution = solve_incremental(pddlstream_problem, planner=planner, max_planner_time=max_planner_time, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True) else: raise ValueError(args.algorithm) print_solution(solution) plan, cost, evaluations = solution pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime if plan is None: return if (not display) or (plan is None): disconnect() return # Maybe openrave didn't actually sample any joints... # http://openrave.org/docs/0.8.2/openravepy/examples.tutorial_iksolutions/ with LockRenderer(): commands = post_process(problem, plan, teleport=teleport) saver.restore() # Assumes bodies are ordered the same way if not args.viewer: disconnect() connect(use_gui=True) with LockRenderer(): with HideOutput(): problem_fn() # TODO: way of doing this without reloading? saver.restore() # Assumes bodies are ordered the same way if args.simulate: control_commands(commands) else: time_step = None if teleport else 0.01 apply_commands(BeliefState(problem), commands, time_step) wait_for_user() 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() # TODO: getopt 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))), ] if USE_SYNTHESIZERS else [] print('Init:', init) print('Goal:', goal) print('Streams:', stream_map.keys()) print('Synthesizers:', stream_map.keys()) print('Names:', names) # initialize and measure performance pr = cProfile.Profile() pr.enable() solution = solve_focused(pddlstream_problem, synthesizers=synthesizers, max_cost=INF) print_solution(solution) plan, cost, evaluations = solution pr.disable() # print stats 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 viewer: # TODO: how to reenable the viewer disconnect() connect(use_gui=True) load_world() else: saved_world.restore() command = postprocess_plan(plan) #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(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): p.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() 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()