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 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 = 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(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 main(time_step=0.01): parser = create_parser() parser.add_argument('-teleport', action='store_true', help='Teleports between configurations') parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') # TODO: argument for selecting prior args = parser.parse_args() print('Arguments:', args) # TODO: nonuniform distribution to bias towards other actions # TODO: closed world and open world real_world = connect(use_gui=not args.viewer) add_data_path() task, state = get_problem1(localized='rooms', p_other=0.25) # surfaces | rooms for body in task.get_bodies(): add_body_name(body) robot = task.robot #dump_body(robot) assert (USE_DRAKE_PR2 == is_drake_pr2(robot)) attach_viewcone(robot) # Doesn't work for the normal pr2? draw_base_limits(get_base_limits(robot), color=(0, 1, 0)) #wait_for_user() # TODO: partially observable values # TODO: base movements preventing pick without look # TODO: do everything in local coordinate frame # TODO: automatically determine an action/command cannot be applied # TODO: convert numpy arrays into what they are close to # TODO: compute whether a plan will still achieve a goal and do that # TODO: update the initial state directly and then just redraw it to ensure uniqueness step = 0 while True: step += 1 print('\n' + 50 * '-') print(step, state) wait_for_user() #print({b: p.value for b, p in state.poses.items()}) with ClientSaver(): commands = plan_commands(state, args) print() if commands is None: print('Failure!') break if not commands: print('Success!') break apply_commands(state, commands, time_step=time_step) print(state) wait_for_user() disconnect()
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 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 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 = 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)
def main(): parser = create_parser() args = parser.parse_args() print('Arguments:', args) problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem tamp_problem = problem_fn() print(tamp_problem) pddlstream_problem = pddlstream_from_tamp(tamp_problem) solution = solve(pddlstream_problem, algorithm=args.algorithm, unit_costs=args.unit) print_solution(solution) plan, cost, evaluations = solution if plan is None: return apply_plan(tamp_problem, plan)
def main(replan=True, defer=True): parser = create_parser() args = parser.parse_args() print('Arguments:', args) 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(problem, algorithm=args.algorithm, unit_costs=args.unit, stream_info=stream_info, replan_actions=replan_actions) print_solution(solution)
def main(): parser = create_parser() #parser.add_argument('-p', '--problem', default='blocked', help='The name of the problem to solve') args = parser.parse_args() print('Arguments:', args) problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem # TODO: use --problem tamp_problem = problem_fn() print(tamp_problem) pddlstream_problem = pddlstream_from_tamp(tamp_problem) #solution = solve_serialized(pddlstream_problem, planner='max-astar', unit_costs=args.unit) solution = solve(pddlstream_problem, algorithm=args.algorithm, unit_costs=args.unit, debug=False, #complexity_step=INF, max_complexity=0, ) print_solution(solution) plan, cost, evaluations = solution if plan is None: return apply_plan(tamp_problem, plan)
def main(): parser = create_parser() 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) connect(use_gui=args.viewer) robot, names, movable = load_world() print('Objects:', names) saver = WorldSaver() problem = pddlstream_from_problem(robot, movable=movable, teleport=args.teleport) _, _, _, stream_map, init, goal = problem print('Init:', init) print('Goal:', goal) print('Streams:', str_from_object(set(stream_map))) with Profiler(): with LockRenderer(lock=not args.enable): solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit, success_cost=INF) saver.restore() print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return command = postprocess_plan(plan) if args.simulate: wait_for_user('Simulate?') command.control() else: wait_for_user('Execute?') #command.step() command.refine(num_steps=10).execute(time_step=0.001) wait_for_user('Finish?') disconnect()
def main(): parser = create_parser() args = parser.parse_args() print('Arguments:', args) problem = get_problem1() print('Init:', problem.init) print('Goal:', problem.goal) info = { # Intentionally, misleading the stream 'increment': StreamInfo(p_success=0.01, overhead=1), 'decrement': StreamInfo(p_success=1, overhead=1), } solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit, stream_info=info, planner='max-astar', effort_weight=1) print_solution(solution)
def main(): parser = create_parser() parser.add_argument('-n', '--n_boxes', default=3, type=int, help='The number of boxes') args = parser.parse_args() print('Arguments:', args) problem = get_problem1(n_boxes=args.n_boxes) print('Init:', sorted(problem.init)) print('Goal:', problem.goal) print(analyze_goal(problem, debug=True, use_actions=True, blocked_predicates=[ # TODO: make sure to use lowercase 'handempty', 'clear', # These are conditions that you can always reachieve? ])) print(SEPARATOR) with Profiler(): solution = solve(problem, algorithm=args.algorithm, unit_costs=args.unit, planner='ff-wastar1', debug=True) print_solution(solution)
def main(): parser = create_parser() parser.add_argument('-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(): solution = solve(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, visualize=args.visualize) print_solution(solution) plan, cost, evaluations = solution step_plan(tamp_problem, plan)
def main(): parser = create_parser() parser.add_argument('-problem', default='problem1', 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(): problem = problem_fn() saver = WorldSaver() draw_base_limits(problem.limits, color=RED) pddlstream_problem = pddlstream_from_problem(problem, collisions=not args.cfree, teleport=args.teleport) stream_info = { 'inverse-kinematics': StreamInfo(), 'plan-base-motion': StreamInfo(overhead=1e1), } _, _, _, 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-astar' search_sample_ratio = 1 max_planner_time = 10 with Profiler(field='cumtime', num=25): # cumtime | tottime with LockRenderer(lock=not args.enable): solution = solve(pddlstream_problem, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, debug=False, unit_efforts=True, effort_weight=1, search_sample_ratio=search_sample_ratio) 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(lock=not args.enable): commands = post_process(problem, plan, teleport=args.teleport) saver.restore() if args.simulate: control_commands(commands) else: time_step = None if args.teleport else 0.01 apply_commands(BeliefState(problem), commands, time_step) wait_for_user() disconnect()
def main(): parser = create_parser(default_algorithm='binding') 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('-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) 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) stream_info = { 'test-cfree-conf-pose': StreamInfo(p_success=1e-2), 'test-cfree-traj-pose': StreamInfo(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 with Profiler(field='tottime', num=25): # cumtime | tottime with LockRenderer(lock=not args.enable): 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) saver.restore() print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return with LockRenderer(): commands = post_process(problem, plan) saver.restore() # Assumes bodies are ordered the same way wait_for_user() if args.simulate: control_commands(commands) else: apply_commands(BeliefState(problem), commands, time_step=1e-2) # 1e-2 | None 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(verbose=True): # TODO: could work just on postprocessing # TODO: try the other reachability database # TODO: option to only consider costs during local optimization parser = create_parser() parser.add_argument('-problem', default='packed', help='The name of the problem to solve') parser.add_argument('-n', '--number', default=5, type=int, help='The number of objects') 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('-teleport', action='store_true', help='Teleports between configurations') parser.add_argument('-enable', action='store_true', help='Enables rendering during planning') 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(): problem = problem_fn(num=args.number) draw_base_limits(problem.base_limits, color=(1, 0, 0)) saver = WorldSaver() #handles = [] #for link in get_group_joints(problem.robot, 'left_arm'): # handles.append(draw_link_name(problem.robot, link)) #wait_for_user() pddlstream_problem = pddlstream_from_problem(problem, collisions=not args.cfree, teleport=args.teleport) stream_info = { 'inverse-kinematics': StreamInfo(), 'plan-base-motion': StreamInfo(overhead=1e1), 'test-cfree-pose-pose': StreamInfo(p_success=1e-3, verbose=verbose), 'test-cfree-approach-pose': StreamInfo(p_success=1e-2, verbose=verbose), 'test-cfree-traj-pose': StreamInfo(p_success=1e-1, verbose=verbose), # TODO: apply to arm and base trajs #'test-cfree-traj-grasp-pose': StreamInfo(verbose=verbose), 'Distance': FunctionInfo(p_success=0.99, opt_fn=lambda q1, q2: BASE_CONSTANT), #'MoveCost': FunctionInfo(lambda t: BASE_CONSTANT), } #stream_info = {} _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) print('Streams:', str_from_object(set(stream_map))) success_cost = 0 if args.optimal else INF planner = 'ff-astar' if args.optimal else 'ff-wastar3' search_sample_ratio = 2 max_planner_time = 10 # effort_weight = 0 if args.optimal else 1 effort_weight = 1e-3 if args.optimal else 1 with Profiler(field='tottime', num=25): # cumtime | tottime with LockRenderer(lock=not args.enable): solution = solve(pddlstream_problem, algorithm=args.algorithm, stream_info=stream_info, planner=planner, max_planner_time=max_planner_time, unit_costs=args.unit, success_cost=success_cost, max_time=args.max_time, verbose=True, debug=False, unit_efforts=True, effort_weight=effort_weight, search_sample_ratio=search_sample_ratio) saver.restore() cost_over_time = [(s.cost, s.time) for s in SOLUTIONS] for i, (cost, runtime) in enumerate(cost_over_time): print('Plan: {} | Cost: {:.3f} | Time: {:.3f}'.format( i, cost, runtime)) #print(SOLUTIONS) print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return with LockRenderer(lock=not args.enable): commands = post_process(problem, plan, teleport=args.teleport) saver.restore() draw_base_limits(problem.base_limits, color=(1, 0, 0)) wait_for_user() if args.simulate: control_commands(commands) else: time_step = None if args.teleport else 0.01 apply_commands(State(), commands, time_step) wait_for_user() disconnect()
def main(): # TODO: side grasps (horizontal gripper, one finger, forklift) parser = create_parser() parser.add_argument('-g', '--gurobi', action='store_true', help='Uses gurobi') 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') tamp_problem, args = initialize(parser) # TODO: test if placed in the same region defer_fn = defer_shared # never_defer | defer_unique | defer_shared stream_info = { 's-region': StreamInfo(defer_fn=defer_fn), 's-grasp': StreamInfo(defer_fn=defer_fn), 's-ik': StreamInfo(defer_fn=get_defer_all_unbound( inputs='?g')), # defer_fn | defer_unbound 's-motion': StreamInfo(defer_fn=get_defer_any_unbound()), 't-cfree': StreamInfo(defer_fn=get_defer_any_unbound(), eager=False, verbose=False), # defer_fn | defer_unbound 't-region': StreamInfo(eager=True, p_success=0), # bound_fn is None 'dist': FunctionInfo(eager=False, defer_fn=get_defer_any_unbound(), opt_fn=lambda q1, q2: MOVE_COST), 'gurobi-cfree': StreamInfo( eager=False, negate=True ), # TODO: AttributeError: 'tuple' object has no attribute 'instance' #'gurobi': OptimizerInfo(p_success=0), #'rrt': OptimizerInfo(p_success=0), } #stream_info = {} hierarchy = [ #ABSTRIPSLayer(pos_pre=['atconf']), #, horizon=1), ] skeletons = [TIGHT_SKELETON] if args.skeleton else None assert implies(args.skeleton, args.problem == 'tight') max_cost = INF # 8*MOVE_COST constraints = PlanConstraints( skeletons=skeletons, #skeletons=[], #skeletons=[skeleton, []], exact=True, max_cost=max_cost) replan_actions = set() #replan_actions = {'move', 'pick', 'place'} pddlstream_problem = pddlstream_from_tamp(tamp_problem, collisions=not args.cfree, use_stream=not args.gurobi, use_optimizer=args.gurobi) dump_pddlstream(pddlstream_problem) success_cost = 0 if args.optimal else INF #planner = 'dijkstra' planner = 'max-astar' #planner = 'ff-wastar1' #effort_weight = 1. effort_weight = 1. / get_cost_scale() #effort_weight = None with Profiler(field='cumtime', num=20): solution = solve(pddlstream_problem, algorithm=args.algorithm, constraints=constraints, stream_info=stream_info, replan_actions=replan_actions, planner=planner, max_planner_time=10, hierarchy=hierarchy, max_time=args.max_time, max_iterations=INF, debug=False, verbose=True, unit_costs=args.unit, success_cost=success_cost, unit_efforts=True, effort_weight=effort_weight, search_sample_ratio=1, visualize=args.visualize) # TODO: solve_serialized print_solution(solution) plan, cost, evaluations = solution if plan is not None: display_plan(tamp_problem, retime_plan(plan))
def main(partial=False, defer=False, verbose=True): parser = create_parser() 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) 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() saver = WorldSaver() #dump_world() pddlstream_problem = pddlstream_from_problem(problem, teleport=args.teleport) stream_info = { # 'test-cfree-pose-pose': StreamInfo(p_success=1e-3, verbose=verbose), # 'test-cfree-approach-pose': StreamInfo(p_success=1e-2, verbose=verbose), # 'test-cfree-traj-pose': StreamInfo(p_success=1e-1, verbose=verbose), 'MoveCost': FunctionInfo(opt_move_cost_fn), } stream_info.update({ '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), } 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)), }) _, _, _, stream_map, init, goal = pddlstream_problem print('Init:', init) print('Goal:', goal) print('Streams:', str_from_object(set(stream_map))) print(SEPARATOR) with Profiler(): with LockRenderer(lock=not args.enable): solution = solve(pddlstream_problem, algorithm=args.algorithm, unit_costs=args.unit, stream_info=stream_info, success_cost=INF, verbose=True, debug=False) saver.restore() print_solution(solution) plan, cost, evaluations = solution if (plan is None) or not has_gui(): disconnect() return print(SEPARATOR) with LockRenderer(lock=not args.enable): commands = post_process(problem, plan) problem.remove_gripper() saver.restore() #restore_state(state_id) saver.restore() wait_if_gui('Execute?') if args.simulate: control_commands(commands) else: apply_commands(State(), commands, time_step=0.01) wait_if_gui('Finish?') disconnect()