def collect_scoop(args, ros_world): arm, _ = ACTIVE_ARMS['scoop'] cup_name, bowl_name = get_scoop_items(ros_world) spoon_name = create_name(args.spoon, 1) # grey_spoon | orange_spoon | green_spoon init_holding = get_spoon_init_holding(arm, spoon_name) init = [ ('Contains', bowl_name, SUGAR), ] goal = [ ('Contains', cup_name, SUGAR), ] skeleton = [ ('move-arm', [arm, X, X, X]), ('scoop', [arm, bowl_name, X, spoon_name, X, SUGAR, X, X, X]), ('move-arm', [arm, X, X, X]), ('pour', [arm, cup_name, X, spoon_name, X, SUGAR, X, X, X]), ('move-arm', [arm, X, X, X]), ] constraints = PlanConstraints(skeletons=[skeleton], exact=True) task = Task(init=init, init_holding=init_holding, goal=goal, arms=[arm], required=[cup_name, bowl_name], reset_arms=True, use_scales=True, constraints=constraints) add_holding(task, ros_world) feature = get_scoop_feature(ros_world, bowl_name, spoon_name) return task, feature
def solve_current(problem, constraints=PlanConstraints(), unit_costs=False, verbose=False, **search_args): """ Solves a PDDLStream problem without applying any streams Will fail if the problem requires stream applications :param problem: a PDDLStream problem :param constraints: PlanConstraints on the available solutions :param unit_costs: use unit action costs rather than numeric costs :param verbose: if True, this prints the result of each stream application :param search_args: keyword args for the search subroutine :return: a tuple (plan, cost, evaluations) where plan is a sequence of actions (or None), cost is the cost of the plan, and evaluations is init but expanded using stream applications """ evaluations, goal_expression, domain, externals = parse_problem( problem, constraints=constraints, unit_costs=unit_costs) instantiator = Instantiator(externals, evaluations) process_function_queue(instantiator, evaluations, verbose=verbose) plan, cost = solve_finite(evaluations, goal_expression, domain, max_cost=constraints.max_cost, **search_args) return revert_solution(plan, cost, evaluations)
def examine_instantiated(problem, constraints=PlanConstraints(), unit_costs=False, unique=False, verbose=False, debug=False): # TODO: refactor to an analysis file domain_pddl, constant_map, stream_pddl, _, init, goal = problem stream_map = DEBUG if unique else SHARED_DEBUG # DEBUG_MODES problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal) evaluations, goal_exp, domain, externals = parse_problem( problem, constraints=constraints, unit_costs=unit_costs) assert not isinstance(domain, SimplifiedDomain) # store = SolutionStore(evaluations, max_time, success_cost=INF, verbose=verbose) # instantiator = Instantiator(externals, evaluations) # process_stream_queue(instantiator, store, complexity_limit=INF, verbose=verbose) # results = [] # TODO: extract from process_stream_queue #set_unique(externals) # domain.actions[:] = [] # TODO: only instantiate axioms # TODO: drop all fluents and instantiate # TODO: relaxed planning version of this results, exhausted = optimistic_process_streams(evaluations, externals, complexity_limit=INF, max_effort=None) evaluations = evaluations_from_stream_plan(evaluations, results, max_effort=None) problem = get_problem(evaluations, goal_exp, domain, unit_costs) task = task_from_domain_problem(domain, problem) with Verbose(debug): instantiated = instantiate_task(task) if instantiated is None: return None # TODO: reinstantiate actions? instantiated.axioms[:] = [reinstantiate_axiom(axiom) for axiom in instantiated.axioms] instantiated = convert_instantiated(instantiated) return results, instantiated
def collect_pour(args, ros_world): arm, _ = ACTIVE_ARMS['pour'] cup_name, bowl_name = get_pour_items(ros_world) init = [ ('Contains', cup_name, COFFEE), ] goal = [ ('Contains', bowl_name, COFFEE), ] skeleton = [ ('move-arm', [arm, X, X, X]), ('pick', [arm, cup_name, X, X, X, X, X]), ('move-arm', [arm, X, X, X]), ('pour', [arm, bowl_name, X, cup_name, X, COFFEE, X, X, X]), ('move-arm', [arm, X, X, X]), ('place', [arm, cup_name, X, X, X, X, X, X, X, X]), ('move-arm', [arm, X, X, X]), ] constraints = PlanConstraints(skeletons=[skeleton], exact=True) # TODO: remove required # TODO: ensure the robot reaches its start configuration task = Task( init=init, goal=goal, arms=[arm], required=[cup_name, bowl_name], # TODO: table? reset_arms=True, empty_arms=True, use_scales=True, constraints=constraints) feature = get_pour_feature(ros_world, bowl_name, cup_name) return task, feature
def compute_skeleton_constraints(action_plan, bindings): skeleton = [] groups = { arg: values for arg, values in bindings.items() if len(values) != 1 } for name, args in action_plan: new_args = [] for arg in args: if isinstance(arg, Object): new_args.append(arg) elif isinstance(arg, OptimisticObject): assert bindings.get(arg, []) if len(bindings[arg]) == 1: new_args.append(bindings[arg][0]) else: #new_args.append(WILD) new_args.append(arg) else: raise ValueError(arg) skeleton.append((name, new_args)) # exact=False because we might need new actions return PlanConstraints(skeletons=[skeleton], groups=groups, exact=False, max_cost=INF)
def compute_skeleton_constraints(opt_plan, bindings): skeleton = [] groups = { arg: values for arg, values in bindings.items() if len(values) != 1 } action_plan, preimage_facts = opt_plan for name, args in action_plan: new_args = [] for arg in args: if isinstance(arg, Object): new_args.append(arg) elif isinstance(arg, OptimisticObject): new_args.append(WILD) # TODO: might cause some strange effects on continuous_tamp -p blocked #assert bindings.get(arg, []) #if len(bindings[arg]) == 1: # new_args.append(bindings[arg][0]) #else: # #new_args.append(WILD) # new_args.append(arg) else: raise ValueError(arg) skeleton.append((name, new_args)) # exact=False because we might need new actions return PlanConstraints(skeletons=[skeleton], groups=groups, exact=False, max_cost=INF)
def examine_instantiated(problem, constraints=PlanConstraints(), unit_costs=False, max_time=INF, verbose=False, **search_args): domain_pddl, constant_map, stream_pddl, _, init, goal = problem stream_map = DEBUG problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal) evaluations, goal_exp, domain, externals = parse_problem( problem, constraints=constraints, unit_costs=unit_costs) store = SolutionStore(evaluations, max_time, success_cost=INF, verbose=verbose) #externals = compile_fluents_as_attachments(domain, externals) # instantiator = Instantiator(externals, evaluations) process_stream_queue(instantiator, store, complexity_limit=INF, verbose=verbose) #plan, cost = solve_finite(evaluations, goal_exp, domain, max_cost=max_cost, **search_args) debug = False assert not isinstance(domain, SimplifiedDomain) problem = get_problem(evaluations, goal_exp, domain, unit_costs) task = task_from_domain_problem(domain, problem) with Verbose(debug): instantiated = instantiate_task(task) instantiated = convert_instantiated(instantiated) return instantiated
def collect_push(args, ros_world): arm = LEFT_ARM block_name, = get_pour_items(ros_world) goal_pos2d = np.random.uniform(*POSE2D_RANGE)[:2] init = [('CanPush', block_name, goal_pos2d)] goal = [('InRegion', block_name, goal_pos2d)] skeleton = [ ('move-arm', [arm, X, X, X]), ('push', [arm, block_name, X, X, X, X, X]), ('move-arm', [arm, X, X, X]), ] constraints = PlanConstraints(skeletons=[skeleton], exact=True) task = Task(init=init, goal=goal, arms=[arm], required=[block_name], reset_arms=True, use_scales=False, constraints=constraints) initial_pose = ros_world.perception.get_pose(block_name) feature = get_push_feature(ros_world, block_name, arm, initial_pose, goal_pos2d) for goal in task.goal: if goal[0] == 'InRegion': _, block_name, goal_pos2d = goal draw_push_goal(ros_world, block_name, goal_pos2d) return task, feature
def solve_incremental(problem, constraints=PlanConstraints(), unit_costs=False, success_cost=INF, max_iterations=INF, max_time=INF, start_complexity=0, complexity_step=1, max_complexity=INF, verbose=False, **search_args): """ Solves a PDDLStream problem by alternating between applying all possible streams and searching :param problem: a PDDLStream problem :param constraints: PlanConstraints on the set of legal solutions :param max_time: the maximum amount of time to apply streams :param max_iterations: the maximum amount of search iterations :param unit_costs: use unit action costs rather than numeric costs :param success_cost: an exclusive (strict) upper bound on plan cost to terminate :param start_complexity: the stream complexity on the first iteration :param complexity_step: the increase in the complexity limit after each iteration :param max_complexity: the maximum stream complexity :param verbose: if True, this prints the result of each stream application :param search_args: keyword args for the search subroutine :return: a tuple (plan, cost, evaluations) where plan is a sequence of actions (or None), cost is the cost of the plan, and evaluations is init but expanded using stream applications """ # max_complexity = 0 => current # complexity_step = INF => exhaustive # success_cost = terminate_cost = decision_cost evaluations, goal_expression, domain, externals = parse_problem( problem, constraints=constraints, unit_costs=unit_costs) store = SolutionStore(evaluations, max_time, success_cost, verbose) # TODO: include other info here? ensure_no_fluent_streams(externals) if UPDATE_STATISTICS: load_stream_statistics(externals) num_iterations = num_calls = 0 complexity_limit = start_complexity instantiator = Instantiator(externals, evaluations) num_calls += process_stream_queue(instantiator, store, complexity_limit, verbose=verbose) while not store.is_terminated() and (num_iterations <= max_iterations) and (complexity_limit <= max_complexity): num_iterations += 1 print('Iteration: {} | Complexity: {} | Calls: {} | Evaluations: {} | Solved: {} | Cost: {} | Time: {:.3f}'.format( num_iterations, complexity_limit, num_calls, len(evaluations), store.has_solution(), store.best_cost, store.elapsed_time())) plan, cost = solve_finite(evaluations, goal_expression, domain, max_cost=min(store.best_cost, constraints.max_cost), **search_args) if is_plan(plan): store.add_plan(plan, cost) if not instantiator: break if complexity_step is None: # TODO: option to select the next k-smallest complexities complexity_limit = instantiator.min_complexity() else: complexity_limit += complexity_step num_calls += process_stream_queue(instantiator, store, complexity_limit, verbose=verbose) #retrace_stream_plan(store, domain, goal_expression) #print('Final queue size: {}'.format(len(instantiator))) if UPDATE_STATISTICS: write_stream_statistics(externals, verbose) return store.extract_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 __init__(self, init=[], goal=[], init_holding={}, arms=[LEFT_ARM], required=[], stackable=[], graspable=[], pushable=[], stirrable=[], can_scoop=[], scoopable=[], pressable=[], pourable=[], can_contain=[], use_kitchen=False, use_scales=False, reset_arms=False, empty_arms=False, reset_items=False, constraints=PlanConstraints()): self.init = init self.goal = goal self.arms = arms self.init_holding = init_holding self.required = set(required) self.stackable = [TABLE] + list(stackable) self.graspable = ['greenblock', 'cup'] + list( graspable) # 'spoon', 'stirrer' self.pushable = ['purpleblock'] + list(pushable) self.stirrable = ['spoon', 'stirrer'] + list(stirrable) self.can_scoop = ['spoon'] + list(can_scoop) self.scoopable = ['bowl'] + list(scoopable) self.pressable = ['button'] + list(pressable) self.can_contain = ['bowl'] + list(can_contain) # 'cup' self.pourable = ['cup', 'spoon'] + list(pourable) # 'bowl' self.use_kitchen = use_kitchen self.use_scales = use_scales self.reset_arms = reset_arms self.empty_arms = select_values(empty_arms, self.arms) self.reset_items = select_values(reset_items, self.graspable) self.constraints = constraints
def solve_exhaustive(problem, constraints=PlanConstraints(), unit_costs=False, max_time=300, verbose=False, **search_args): """ Solves a PDDLStream problem by applying all possible streams and searching once Requires a finite max_time when infinitely many stream instances :param problem: a PDDLStream problem :param constraints: PlanConstraints on the available solutions :param unit_costs: use unit action costs rather than numeric costs :param max_time: the maximum amount of time to apply streams :param verbose: if True, this prints the result of each stream application :param search_args: keyword args for the search subroutine :return: a tuple (plan, cost, evaluations) where plan is a sequence of actions (or None), cost is the cost of the plan, and evaluations is init but expanded using stream applications """ start_time = time.time() evaluations, goal_expression, domain, externals = parse_problem( problem, constraints=constraints, unit_costs=unit_costs) ensure_no_fluent_streams(externals) if UPDATE_STATISTICS: load_stream_statistics(externals) instantiator = Instantiator(externals, evaluations) while instantiator.stream_queue and (elapsed_time(start_time) < max_time): process_instance(instantiator, evaluations, instantiator.pop_stream(), verbose=verbose) process_function_queue(instantiator, evaluations, verbose=verbose) plan, cost = solve_finite(evaluations, goal_expression, domain, max_cost=constraints.max_cost, **search_args) if UPDATE_STATISTICS: write_stream_statistics(externals, verbose) return revert_solution(plan, cost, evaluations)
def main(): parser = argparse.ArgumentParser() parser.add_argument('-a', '--algorithm', default='focused', help='Specifies the algorithm') 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') # TODO: test if placed in the same region defer_fn = defer_shared # never_defer | defer_unique | defer_shared tamp_problem, args = initialize(parser) 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, negate=True), # defer_fn | defer_unbound 't-region': StreamInfo(eager=False, p_success=0), # bound_fn is None 'dist': FunctionInfo(defer_fn=get_defer_any_unbound(), opt_fn=lambda q1, q2: MOVE_COST), 'gurobi-cfree': StreamInfo(eager=False, negate=True), #'gurobi': OptimizerInfo(p_success=0), #'rrt': OptimizerInfo(p_success=0), } 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) pr = cProfile.Profile() pr.enable() success_cost = 0 if args.optimal else INF planner = 'max-astar' #planner = 'ff-wastar1' if args.algorithm == 'focused': solver = solve_focused # solve_focused | solve_serialized solution = solver( pddlstream_problem, constraints=constraints, stream_info=stream_info, replan_actions=replan_actions, planner=planner, 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=True, effort_weight=1, search_sample_ratio=1, #max_skeletons=None, bind=True, visualize=args.visualize) elif args.algorithm == 'incremental': solution = solve_incremental(pddlstream_problem, constraints=constraints, complexity_step=2, planner=planner, 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('cumtime').print_stats(20) if plan is not None: display_plan(tamp_problem, retime_plan(plan))
def solve_pddlstream(belief, problem, args, skeleton=None, replan_actions=set(), max_time=INF, max_memory=MAX_MEMORY, max_cost=INF): set_cost_scale(COST_SCALE) reset_globals() stream_info = get_stream_info() #print(set(stream_map) - set(stream_info)) skeletons = create_ordered_skeleton(skeleton) max_cost = min(max_cost, COST_BOUND) print('Max cost: {:.3f} | Max runtime: {:.3f}'.format(max_cost, max_time)) constraints = PlanConstraints(skeletons=skeletons, max_cost=max_cost, exact=True) success_cost = 0 if args.anytime else INF planner = 'ff-astar' if args.anytime else 'ff-wastar2' search_sample_ratio = 0.5 # 0.5 max_planner_time = 10 # TODO: max number of samples per iteration flag # TODO: don't greedily expand samples with too high of a complexity if out of time pr = cProfile.Profile() pr.enable() saver = WorldSaver() sim_state = belief.sample_state() sim_state.assign() wait_for_duration(0.1) with LockRenderer(lock=not args.visualize): # TODO: option to only consider costs during local optimization # effort_weight = 0 if args.anytime else 1 effort_weight = 1e-3 if args.anytime else 1 #effort_weight = 0 #effort_weight = None solution = solve_focused( problem, constraints=constraints, stream_info=stream_info, replan_actions=replan_actions, initial_complexity=5, planner=planner, max_planner_time=max_planner_time, unit_costs=args.unit, success_cost=success_cost, max_time=max_time, max_memory=max_memory, verbose=True, debug=False, unit_efforts=True, effort_weight=effort_weight, max_effort=INF, # bind=True, max_skeletons=None, search_sample_ratio=search_sample_ratio) saver.restore() # print([(s.cost, s.time) for s in SOLUTIONS]) # print(SOLUTIONS) print_solution(solution) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime return solution
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(): 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('-g', '--gurobi', action='store_true', help='Uses gurobi') parser.add_argument('-n', '--number', default=2, type=int, help='The number of blocks') 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') parser.add_argument('-t', '--max_time', default=30, type=int, help='The max time') parser.add_argument('-u', '--unit', action='store_true', help='Uses unit costs') parser.add_argument('-v', '--visualize', action='store_true', help='Visualizes graphs') args = parser.parse_args() print('Arguments:', args) np.set_printoptions(precision=2) if args.deterministic: random.seed(seed=0) np.random.seed(seed=0) print('Random seed:', get_random_seed()) problem_from_name = {fn.__name__: fn for fn in PROBLEMS} if args.problem not in problem_from_name: raise ValueError(args.problem) print('Problem:', args.problem) problem_fn = problem_from_name[args.problem] tamp_problem = problem_fn(args.number) print(tamp_problem) action_info = { #'move': ActionInfo(terminal=True), #'pick': ActionInfo(terminal=True), #'place': ActionInfo(terminal=True), } stream_info = { 't-region': StreamInfo(eager=False, p_success=0), # bound_fn is None 't-cfree': StreamInfo(eager=False, negate=True), 'distance': FunctionInfo(opt_fn=lambda q1, q2: MOVE_COST), 'gurobi-cfree': StreamInfo(eager=False, negate=True), #'gurobi': OptimizerInfo(p_success=0), #'rrt': OptimizerInfo(p_success=0), } hierarchy = [ #ABSTRIPSLayer(pos_pre=['atconf']), #, horizon=1), ] skeletons = [TIGHT_SKELETON] if args.skeleton else None max_cost = INF # 8*MOVE_COST constraints = PlanConstraints( skeletons=skeletons, #skeletons=[], #skeletons=[skeleton, []], exact=True, max_cost=max_cost) pddlstream_problem = pddlstream_from_tamp(tamp_problem, collisions=not args.cfree, use_stream=not args.gurobi, use_optimizer=args.gurobi) print('Initial:', sorted_str_from_list(pddlstream_problem.init)) print('Goal:', str_from_object(pddlstream_problem.goal)) pr = cProfile.Profile() pr.enable() success_cost = 0 if args.optimal else INF planner = 'max-astar' #planner = 'ff-wastar1' if args.algorithm == 'focused': solution = solve_focused( pddlstream_problem, constraints=constraints, action_info=action_info, stream_info=stream_info, planner=planner, 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=1, #max_skeletons=None, bind=True, visualize=args.visualize) elif args.algorithm == 'incremental': solution = solve_incremental(pddlstream_problem, constraints=constraints, complexity_step=2, planner=planner, 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('cumtime').print_stats(20) if plan is not None: display_plan(tamp_problem, retime_plan(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)
def solve(problem, algorithm=DEFAULT_ALGORITHM, constraints=PlanConstraints(), stream_info={}, replan_actions=set(), unit_costs=False, success_cost=INF, max_time=INF, max_iterations=INF, max_memory=INF, initial_complexity=0, complexity_step=1, max_complexity=INF, max_skeletons=INF, search_sample_ratio=1, max_failures=0, unit_efforts=False, max_effort=INF, effort_weight=None, reorder=True, #temp_dir=TEMP_DIR, clean=False, debug=False, hierarchy=[], #planner=DEFAULT_PLANNER, max_planner_time=DEFAULT_MAX_TIME, max_cost=INF, debug=False visualize=False, verbose=True, **search_kwargs): """ Solves a PDDLStream problem generically using one of the available algorithms :param problem: a PDDLStream problem :param algorithm: a PDDLStream algorithm name :param constraints: PlanConstraints on the set of legal solutions :param stream_info: a dictionary from stream name to StreamInfo altering how individual streams are handled :param replan_actions: the actions declared to induce replanning for the purpose of deferred stream evaluation :param unit_costs: use unit action costs rather than numeric costs :param success_cost: the exclusive (strict) upper bound on plan cost to successfully terminate :param max_time: the maximum runtime :param max_iterations: the maximum number of search iterations :param max_memory: the maximum amount of memory :param initial_complexity: the initial stream complexity limit :param complexity_step: the increase in the stream complexity limit per iteration :param max_complexity: the maximum stream complexity limit :param max_skeletons: the maximum number of plan skeletons (max_skeletons=None indicates not adaptive) :param search_sample_ratio: the desired ratio of sample time / search time when max_skeletons!=None :param max_failures: the maximum number of stream failures before switching phases when max_skeletons=None :param unit_efforts: use unit stream efforts rather than estimated numeric efforts :param max_effort: the maximum amount of stream effort :param effort_weight: a multiplier for stream effort compared to action costs :param reorder: if True, reorder stream plans to minimize the expected sampling overhead :param visualize: if True, draw the constraint network and stream plan as a graphviz file :param verbose: if True, print the result of each stream application :param search_kwargs: keyword args for the search subroutine :return: a tuple (plan, cost, evaluations) where plan is a sequence of actions (or None), cost is the cost of the plan (INF if no plan), and evaluations is init expanded using stream applications """ # TODO: print the arguments using locals() # TODO: could instead make common arguments kwargs but then they could have different default values # TODO: portfolios of PDDLStream algorithms if algorithm == 'incremental': return solve_incremental( problem=problem, constraints=constraints, unit_costs=unit_costs, success_cost=success_cost, max_iterations=max_iterations, max_time=max_time, max_memory=max_memory, initial_complexity=initial_complexity, complexity_step=complexity_step, max_complexity=max_complexity, verbose=verbose, **search_kwargs) # if algorithm == 'abstract_focused': # meta_focused | meta_focused # return solve_focused( # problem, constraints=constraints, # stream_info=stream_info, replan_actions=replan_actions, # unit_costs=unit_costs, success_cost=success_cost, # max_time=max_time, max_iterations=max_iterations, max_memory=max_memory, # initial_complexity=initial_complexity, complexity_step=complexity_step, #max_complexity=max_complexity, # max_skeletons=max_skeletons, search_sample_ratio=search_sample_ratio, # bind=bind, max_failures=max_failures, # unit_efforts=unit_efforts, max_effort=max_effort, effort_weight=effort_weight, reorder=reorder, # visualize=visualize, verbose=verbose, **search_kwargs) fail_fast = (max_failures < INF) if algorithm == 'focused': return solve_focused_original( problem, constraints=constraints, stream_info=stream_info, replan_actions=replan_actions, unit_costs=unit_costs, success_cost=success_cost, max_time=max_time, max_iterations=max_iterations, max_memory=max_memory, initial_complexity=initial_complexity, complexity_step=complexity_step, max_complexity=max_complexity, #max_skeletons=max_skeletons, search_sample_ratio=search_sample_ratio, fail_fast=fail_fast, # bind=bind, max_failures=max_failures, unit_efforts=unit_efforts, max_effort=max_effort, effort_weight=effort_weight, reorder=reorder, visualize=visualize, verbose=verbose, **search_kwargs) if algorithm == 'binding': return solve_binding( problem, constraints=constraints, stream_info=stream_info, replan_actions=replan_actions, unit_costs=unit_costs, success_cost=success_cost, max_time=max_time, max_iterations=max_iterations, max_memory=max_memory, initial_complexity=initial_complexity, complexity_step=complexity_step, max_complexity=max_complexity, #max_skeletons=max_skeletons, search_sample_ratio=search_sample_ratio, fail_fast=fail_fast, # bind=bind, max_failures=max_failures, unit_efforts=unit_efforts, max_effort=max_effort, effort_weight=effort_weight, reorder=reorder, visualize=visualize, verbose=verbose, **search_kwargs) if algorithm == 'adaptive': return solve_adaptive( problem, constraints=constraints, stream_info=stream_info, replan_actions=replan_actions, unit_costs=unit_costs, success_cost=success_cost, max_time=max_time, max_iterations=max_iterations, max_memory=max_memory, initial_complexity=initial_complexity, complexity_step=complexity_step, max_complexity=max_complexity, max_skeletons=max_skeletons, search_sample_ratio=search_sample_ratio, #bind=bind, max_failures=max_failures, unit_efforts=unit_efforts, max_effort=max_effort, effort_weight=effort_weight, reorder=reorder, visualize=visualize, verbose=verbose, **search_kwargs) raise NotImplementedError(algorithm)
def solve_incremental(problem, constraints=PlanConstraints(), unit_costs=False, success_cost=INF, max_iterations=INF, max_time=INF, max_memory=INF, initial_complexity=0, complexity_step=1, max_complexity=INF, verbose=False, **search_kwargs): """ Solves a PDDLStream problem by alternating between applying all possible streams and searching :param problem: a PDDLStream problem :param constraints: PlanConstraints on the set of legal solutions :param unit_costs: use unit action costs rather than numeric costs :param success_cost: the exclusive (strict) upper bound on plan cost to successfully terminate :param max_time: the maximum runtime :param max_iterations: the maximum number of search iterations :param max_memory: the maximum amount of memory :param initial_complexity: the initial stream complexity limit :param complexity_step: the increase in the stream complexity limit per iteration :param max_complexity: the maximum stream complexity limit :param verbose: if True, print the result of each stream application :param search_kwargs: keyword args for the search subroutine :return: a tuple (plan, cost, evaluations) where plan is a sequence of actions (or None), cost is the cost of the plan (INF if no plan), and evaluations is init expanded using stream applications """ # max_complexity = 0 => current # complexity_step = INF => exhaustive # success_cost = terminate_cost = decision_cost # TODO: warning if optimizers are present evaluations, goal_expression, domain, externals = parse_problem( problem, constraints=constraints, unit_costs=unit_costs) store = SolutionStore( evaluations, max_time, success_cost, verbose, max_memory=max_memory) # TODO: include other info here? if UPDATE_STATISTICS: load_stream_statistics(externals) static_externals = compile_fluents_as_attachments(domain, externals) num_iterations = num_calls = 0 complexity_limit = initial_complexity instantiator = Instantiator(static_externals, evaluations) num_calls += process_stream_queue(instantiator, store, complexity_limit, verbose=verbose) while not store.is_terminated() and (num_iterations < max_iterations) and ( complexity_limit <= max_complexity): num_iterations += 1 print( 'Iteration: {} | Complexity: {} | Calls: {} | Evaluations: {} | Solved: {} | Cost: {:.3f} | ' 'Search Time: {:.3f} | Sample Time: {:.3f} | Time: {:.3f}'.format( num_iterations, complexity_limit, num_calls, len(evaluations), store.has_solution(), store.best_cost, store.search_time, store.sample_time, store.elapsed_time())) plan, cost = solve_finite(evaluations, goal_expression, domain, max_cost=min(store.best_cost, constraints.max_cost), **search_kwargs) if is_plan(plan): store.add_plan(plan, cost) if not instantiator: break if complexity_step is None: # TODO: option to select the next k-smallest complexities complexity_limit = instantiator.min_complexity() else: complexity_limit += complexity_step num_calls += process_stream_queue(instantiator, store, complexity_limit, verbose=verbose) #retrace_stream_plan(store, domain, goal_expression) #print('Final queue size: {}'.format(len(instantiator))) summary = store.export_summary() summary.update({ 'iterations': num_iterations, 'complexity': complexity_limit, }) print('Summary: {}'.format(str_from_object( summary, ndigits=3))) # TODO: return the summary if UPDATE_STATISTICS: write_stream_statistics(externals, verbose) return store.extract_solution()
def collect_stir(world, num_beads=100): arm = LEFT_ARM # TODO: randomize geometries for stirrer spoon_name = create_name( 'grey_spoon', 1) # green_spoon | grey_spoon | orange_spoon | stirrer bowl_name = create_name('whitebowl', 1) scale_name = create_name('onyx_scale', 1) item_ranges = { spoon_name: InitialRanges( width_range=(1., 1.), height_range=(1., 1.), mass_range=(1., 1.), pose2d_range=([0.3, 0.5, -np.pi / 2], [0.3, 0.5, -np.pi / 2]), surface=TABLE_NAME, ), bowl_name: InitialRanges( width_range=(0.75, 1.25), height_range=(0.75, 1.25), mass_range=(1., 1.), pose2d_range=([0.5, -0.05, -np.pi], [0.6, 0.05, np.pi]), # x, y, theta surface=scale_name, ), } # TODO: make a sandbox on the table to contain the beads ################################################## #alpha = 0.75 alpha = 1 bead_colors = [ (1, 0, 0, alpha), (0, 0, 1, alpha), ] beads_fraction = random.uniform(0.75, 1.25) print('Beads fraction:', beads_fraction) bead_radius = sample_norm(mu=0.006, sigma=0.001, lower=0.004) # 0.007 print('Bead radius:', bead_radius) # TODO: check collisions/feasibility when sampling # TODO: grasps on the blue cup seem off for some reason... with ClientSaver(world.client): #dump_body(world.robot) world.perception.add_surface('sandbox', TABLE_POSE) world.perception.add_surface(scale_name, TABLE_POSE) create_table_bodies(world, item_ranges) #gripper = create_gripper(world.robot, LEFT_ARM) #set_point(gripper, (1, 0, 1)) #wait_for_user() bowl_body = world.get_body(bowl_name) update_world(world, target_body=bowl_body) init_holding = hold_item(world, arm, spoon_name) if init_holding is None: return INFEASIBLE parameters_from_name = randomize_dynamics( world) # TODO: parameters_from_name['bead'] _, (d, h) = approximate_as_cylinder(bowl_body) bowl_area = np.pi * (d / 2.)**2 print('Bowl area:', bowl_area) bead_area = np.pi * bead_radius**2 print('Bead area:', bead_area) num_beads = int(np.ceil(beads_fraction * bowl_area / bead_area)) print('Num beads:', num_beads) num_per_color = int(num_beads / len(bead_colors)) # TODO: randomize bead physics beads_per_color = [ fill_with_beads( world, bowl_name, create_beads(num_beads, bead_radius, uniform_color=color, parameters={})) for color in bead_colors ] world.initial_beads.update( {bead: bowl_body for beads in beads_per_color for bead in beads}) if any(len(beads) != num_per_color for beads in beads_per_color): return INFEASIBLE #wait_for_user() init = [('Contains', bowl_name, COFFEE)] goal = [('Mixed', bowl_name)] skeleton = [ ('move-arm', [arm, X, X, X]), ('stir', [arm, bowl_name, X, spoon_name, X, X, X, X]), #('move-arm', [arm, X, X, X]), ] constraints = PlanConstraints(skeletons=[skeleton], exact=True) task = Task(init=init, goal=goal, arms=[arm], init_holding=init_holding, reset_arms=False, constraints=constraints) # Reset arm to clear the scene # TODO: constrain the plan skeleton within the task feature = get_stir_feature(world, bowl_name, spoon_name) ################################################## # table_body = world.get_body(TABLE_NAME) # dump_body(table_body) # joint = 0 # p.enableJointForceTorqueSensor(table_body, joint, enableSensor=1, physicsClientId=world.client) # stabilize(world) # reaction_force = get_joint_reaction_force(table_body, joint) # print(np.array(reaction_force[:3])/ GRAVITY) perception = world.perception initial_pose = perception.get_pose(bowl_name) bowl_body = perception.get_body(bowl_name) scale_body = perception.get_body(scale_name) with ClientSaver(world.client): initial_distance = compute_dispersion(bowl_body, beads_per_color) initial_mass = read_mass(scale_body) print(initial_mass) def score_fn(plan): assert plan is not None with ClientSaver(world.client): rgb_image = take_image(world, bowl_body, beads_per_color) values = score_image(rgb_image, bead_colors, beads_per_color) final_pose = perception.get_pose(bowl_name) point_distance = get_distance(point_from_pose(initial_pose), point_from_pose(final_pose)) #, norm=2) quat_distance = quat_angle_between(quat_from_pose(initial_pose), quat_from_pose(final_pose)) print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format( point_distance, quat_distance)) with ClientSaver(world.client): all_beads = list(flatten(beads_per_color)) bowl_beads = get_contained_beads(bowl_body, all_beads) fraction_bowl = float( len(bowl_beads)) / len(all_beads) if all_beads else 0 print('In Bowl: {}'.format(fraction_bowl)) with ClientSaver(world.client): final_dispersion = compute_dispersion(bowl_body, beads_per_color) print('Initial Dispersion: {:.3f} | Final Dispersion {:.3f}'.format( initial_distance, final_dispersion)) score = { 'bowl_translation': point_distance, 'bowl_rotation': quat_distance, 'fraction_in_bowl': fraction_bowl, 'initial_dispersion': initial_distance, 'final_dispersion': final_dispersion, 'num_beads': len(all_beads), # Beads per color DYNAMICS: parameters_from_name, } # TODO: include time required for stirring # TODO: change in dispersion #wait_for_user() #_, args = find_unique(lambda a: a[0] == 'stir', plan) #control = args[-1] return score return task, feature, score_fn
def main(): parser = argparse.ArgumentParser() parser.add_argument('-a', '--algorithm', default='focused', help='Specifies the algorithm') parser.add_argument('-s', '--skeleton', action='store_true', help='Enforces skeleton plan constraints') tamp_problem, args = initialize(parser) colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS)) viewer = ContinuousTMPViewer(SUCTION_HEIGHT, tamp_problem.regions, title='Continuous TAMP') draw_state(viewer, tamp_problem.initial, colors) [robot] = tamp_problem.initial.robot_confs blocks = sorted(tamp_problem.initial.block_poses) regions = sorted(set(tamp_problem.regions) - {GROUND_NAME}) print(robot) print(blocks) print(regions) print(tamp_problem) domain_pddl = read(get_file_path(__file__, 'domain.pddl')) stream_pddl = read(get_file_path(__file__, 'stream.pddl')) constant_map = {} stream_map = DEBUG # TODO: reuse same streams stream_info = { 't-region': StreamInfo(eager=False, p_success=0), # bound_fn is None } streams_from_problem = {} for order in permutations(blocks): for region in regions: # Objects could also have separate goals print(SEPARATOR) print('Block order:', order) print('Goal region:', region) #tamp_problem = dual(n_blocks=args.number, goal_regions=[region]) tamp_problem.goal_regions.update({block: region for block in order}) init, goal = create_problem(tamp_problem) problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal) dump_pddlstream(problem) skeleton = create_skeleton(robot, order, home=tamp_problem.goal_conf is not None) print(skeleton) skeletons = [skeleton] #skeletons = None constraints = PlanConstraints(skeletons=skeletons, exact=True) solution = solve_focused(problem, constraints=constraints, stream_info=stream_info, planner='max-astar', max_planner_time=10, debug=False, max_time=args.max_time, verbose=True, unit_costs=True, unit_efforts=True, effort_weight=1) print_solution(solution) plan, cost, evaluations = solution assert plan is not None # TODO: params not sufficient if no output stream or output not in plan streams = set() for name, params in plan: for param in params: value = get_stream_value(param) if isinstance(value, StreamValue): # TODO: propagate recursively streams.add((value.stream, value.inputs)) streams_from_problem[order, region] = streams #print(streams) #user_input() print(SEPARATOR) dump_statistics() print(SEPARATOR) best_problems, best_p = None, -INF for problems in combinations(streams_from_problem, r=2): # Make r a parameter stream_plans = [streams_from_problem[problem] for problem in problems] intersection = set.intersection(*stream_plans) p = p_disjunction(stream_plans, STATS_PER_STREAM) assert 0 <= p <= 1 print('Problems: {} | Intersection: {} | p={:.3f}'.format(problems, len(intersection), p)) #, intersection) if p > best_p: best_problems, best_p = problems, p print('\nBest: {} (p={:.3f})'.format(best_problems, best_p))
def plan_actions(world, use_constraints=False, unit_costs=True, max_time=300, verbose=True, **kwargs): # TODO: return multiple grasps instead of one pr = cProfile.Profile() pr.enable() with ClientSaver(world.client): # TODO: be careful about the table distance table_pose = get_pose(world.get_body(world.get_table())) torso_pose = get_link_pose( world.robot, link_from_name(world.robot, 'torso_lift_link')) torso_in_table = multiply(invert(table_pose), torso_pose) # Torso wrt table: [-0.6, 0.0, 0.33] print('Torso wrt table:', np.array(point_from_pose(torso_in_table)).round(3).tolist()) #wait_for_interrupt() problem = get_pddlstream(world, **kwargs) p_success = 1e-2 eager = True stream_info = { 'ControlPoseCollision': FunctionInfo(p_success=p_success, eager=eager), 'ControlConfCollision': FunctionInfo(p_success=p_success, eager=eager), 'PosePoseCollision': FunctionInfo(p_success=p_success, eager=eager), 'ConfConfCollision': FunctionInfo(p_success=p_success, eager=eager), 'test-reachable': StreamInfo(p_success=0, eager=True), # TODO: these should automatically be last... 'sample-motion': StreamInfo(p_success=1, overhead=100), } # TODO: RuntimeError: Preimage fact ('order', n0, t0) is not achievable! constraints = world.task.constraints if use_constraints else PlanConstraints( ) solution = solve_focused( problem, planner='ff-wastar1', max_time=max_time, unit_costs=unit_costs, unit_efforts=True, effort_weight=1, stream_info=stream_info, # TODO: bug when max_skeletons=None and effort_weight != None max_skeletons=None, bind=True, max_failures=0, search_sample_ratio=0, constraints=constraints, verbose=verbose, debug=False) #solution = solve_incremental(problem, unit_costs=unit_costs, verbose=True) print_solution(solution) plan, cost, evaluations = solution pr.disable() if verbose: pstats.Stats(pr).sort_stats('tottime').print_stats( 10) # cumtime | tottime return plan
def collect_pour(world, bowl_type=None, cup_type=None, randomize=True, **kwargs): arm = LEFT_ARM bowl_type = random.choice(POUR_BOWLS) if bowl_type is None else bowl_type cup_type = random.choice(POUR_CUPS) if cup_type is None else cup_type # TODO: could directly randomize base_diameter and top_diameter scale = int(randomize) cup_name = create_name(cup_type, 1) bowl_name = create_name(bowl_type, 1) item_ranges = { cup_name: InitialRanges( width_range=interval(extent=0.2 * scale), # the cups are already fairly small #height_range=(0.9, 1.1), height_range=(1.0, 1.2), # Difficult to grasp when this shrinks mass_range=interval(extent=0.2 * scale), pose2d_range=([0.5, 0.3, -np.pi], [0.5, 0.3, np.pi]), # x, y, theta ), bowl_name: InitialRanges( width_range=interval(extent=0.4 * scale), height_range=interval(extent=0.4 * scale), mass_range=interval(extent=0.4 * scale), pose2d_range=([0.5, 0.0, -np.pi], [0.5, 0.0, np.pi]), # x, y, theta ), } if 'item_ranges' in kwargs: for item in kwargs['item_ranges']: item_ranges[item] = kwargs['item_ranges'][item] #dump_ranges(POUR_CUPS, item_ranges[cup_name]) #dump_ranges(POUR_BOWLS, item_ranges[bowl_name]) #for name, limits in item_ranges.items(): # lower, upper = aabb_from_points(read_obj(get_body_obj(name))) # extents = upper - lower # print(name, 'width', (extents[0]*np.array(limits.width_range)).tolist()) # print(name, 'height', (extents[2]*np.array(limits.height_range)).tolist()) # TODO: check collisions/feasibility when sampling cup_fraction = random.uniform(0.75, 1.0) print('Cup fraction:', cup_fraction) bowl_fraction = random.uniform(1.0, 1.0) print('Bowl fraction:', bowl_fraction) bead_radius = sample_norm(mu=0.006, sigma=0.001 * scale, lower=0.004) print('Bead radius:', bead_radius) num_beads = 100 with ClientSaver(world.client): create_table_bodies(world, item_ranges, randomize=randomize) bowl_body = world.get_body(bowl_name) cup_body = world.get_body(cup_name) update_world(world, bowl_body) if body_pair_collision(cup_body, bowl_body): # TODO: automatically try all pairs of bodies return INFEASIBLE if not has_grasp(world, cup_name): return INFEASIBLE # TODO: flatten and only store bowl vs cup parameters_from_name = randomize_dynamics(world, randomize=randomize) parameters_from_name['bead'] = sample_bead_parameters( ) # bead restitution is the most important with LockRenderer(): all_beads = create_beads(num_beads, bead_radius, parameters=parameters_from_name['bead']) bowl_beads = fill_with_beads(world, bowl_name, all_beads, reset_contained=True, height_fraction=bowl_fraction) init_beads = fill_with_beads(world, cup_name, bowl_beads, reset_contained=False, height_fraction=cup_fraction) #wait_for_user() init_mass = sum(map(get_mass, init_beads)) print('Init beads: {} | Init mass: {:.3f}'.format( len(init_beads), init_mass)) if len(init_beads) < MIN_BEADS: return INFEASIBLE world.initial_beads.update({bead: cup_body for bead in init_beads}) latent = { 'num_beads': len(init_beads), 'total_mass': init_mass, 'bead_radius': bead_radius, DYNAMICS: parameters_from_name, } init = [('Contains', cup_name, COFFEE)] goal = [('Contains', bowl_name, COFFEE)] skeleton = [ ('move-arm', [arm, X, X, X]), ('pick', [arm, cup_name, X, X, X, X, X]), ('move-arm', [arm, X, X, X]), ('pour', [arm, bowl_name, X, cup_name, X, COFFEE, X, X, X]), ] constraints = PlanConstraints(skeletons=[skeleton], exact=True) task = Task(init=init, goal=goal, arms=[arm], constraints=constraints) #, init_holding=init_holding) ################################################## feature = get_pour_feature(world, bowl_name, cup_name) #feature['ranges'] = POUR_FEATURE_RANGES initial_pose = world.get_pose(bowl_name) def score_fn(plan): assert plan is not None final_pose = world.get_pose(bowl_name) point_distance = get_distance(point_from_pose(initial_pose), point_from_pose(final_pose)) #, norm=2) quat_distance = quat_angle_between(quat_from_pose(initial_pose), quat_from_pose(final_pose)) print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format( point_distance, quat_distance)) with ClientSaver(world.client): # TODO: lift the bowl up (with particles around) to prevent scale detections final_bowl_beads = get_contained_beads(bowl_body, init_beads) fraction_bowl = safe_ratio(len(final_bowl_beads), len(init_beads), undefined=0) mass_in_bowl = sum(map(get_mass, final_bowl_beads)) final_cup_beads = get_contained_beads(cup_body, init_beads) fraction_cup = safe_ratio(len(final_cup_beads), len(init_beads), undefined=0) mass_in_cup = sum(map(get_mass, final_cup_beads)) print('In Bowl: {} | In Cup: {}'.format(fraction_bowl, fraction_cup)) score = { # Displacements 'bowl_translation': point_distance, 'bowl_rotation': quat_distance, # Masses 'mass_in_bowl': mass_in_bowl, 'mass_in_cup': mass_in_cup, # Counts 'bowl_beads': len(final_bowl_beads), 'cup_beads': len(final_cup_beads), # Fractions 'fraction_in_bowl': fraction_bowl, 'fraction_in_cup': fraction_cup, } score.update(latent) # TODO: store the cup path length to bias towards shorter paths #_, args = find_unique(lambda a: a[0] == 'pour', plan) #control = args[-1] #feature = control['feature'] #parameter = control['parameter'] return score return task, feature, score_fn
def solve_focused(problem, constraints=PlanConstraints(), stream_info={}, action_info={}, synthesizers=[], max_time=INF, max_iterations=INF, max_skeletons=INF, unit_costs=False, success_cost=INF, complexity_step=1, unit_efforts=False, max_effort=INF, effort_weight=None, reorder=True, search_sample_ratio=0, visualize=False, verbose=True, **search_kwargs): """ Solves a PDDLStream problem by first hypothesizing stream outputs and then determining whether they exist :param problem: a PDDLStream problem :param constraints: PlanConstraints on the set of legal solutions :param stream_info: a dictionary from stream name to StreamInfo altering how individual streams are handled :param action_info: a dictionary from stream name to ActionInfo for planning and execution :param synthesizers: a list of StreamSynthesizer objects :param max_time: the maximum amount of time to apply streams :param max_iterations: the maximum number of search iterations :param max_iterations: the maximum number of plan skeletons to consider :param unit_costs: use unit action costs rather than numeric costs :param success_cost: an exclusive (strict) upper bound on plan cost to terminate :param unit_efforts: use unit stream efforts rather than estimated numeric efforts :param complexity_step: the increase in the effort limit after each failure :param max_effort: the maximum amount of effort to consider for streams :param effort_weight: a multiplier for stream effort compared to action costs :param reorder: if True, stream plans are reordered to minimize the expected sampling overhead :param search_sample_ratio: the desired ratio of search time / sample time :param visualize: if True, it draws the constraint network and stream plan as a graphviz file :param verbose: if True, this prints the result of each stream application :param search_kwargs: keyword args for the search subroutine :return: a tuple (plan, cost, evaluations) where plan is a sequence of actions (or None), cost is the cost of the plan, and evaluations is init but expanded using stream applications """ # TODO: select whether to search or sample based on expected success rates # TODO: no optimizers during search with relaxed_stream_plan num_iterations = search_time = sample_time = eager_calls = 0 complexity_limit = float(INITIAL_COMPLEXITY) eager_disabled = effort_weight is None # No point if no stream effort biasing evaluations, goal_exp, domain, externals = parse_problem( problem, stream_info=stream_info, constraints=constraints, unit_costs=unit_costs, unit_efforts=unit_efforts) store = SolutionStore(evaluations, max_time, success_cost, verbose) full_action_info = get_action_info(action_info) load_stream_statistics(externals + synthesizers) if visualize and not has_pygraphviz(): visualize = False print('Warning, visualize=True requires pygraphviz. Setting visualize=False') if visualize: reset_visualizations() streams, functions, negative = partition_externals(externals, verbose=verbose) eager_externals = list(filter(lambda e: e.info.eager, externals)) skeleton_queue = SkeletonQueue(store, goal_exp, domain) disabled = set() while (not store.is_terminated()) and (num_iterations < max_iterations): start_time = time.time() num_iterations += 1 eager_instantiator = Instantiator(eager_externals, evaluations) # Only update after an increase? if eager_disabled: push_disabled(eager_instantiator, disabled) eager_calls += process_stream_queue(eager_instantiator, store, complexity_limit=complexity_limit, verbose=verbose) print('\nIteration: {} | Complexity: {} | Skeletons: {} | Skeleton Queue: {} | Disabled: {} | Evaluations: {} | ' 'Eager Calls: {} | Cost: {:.3f} | Search Time: {:.3f} | Sample Time: {:.3f} | Total Time: {:.3f}'.format( num_iterations, complexity_limit, len(skeleton_queue.skeletons), len(skeleton_queue), len(disabled), len(evaluations), eager_calls, store.best_cost, search_time, sample_time, store.elapsed_time())) optimistic_solve_fn = get_optimistic_solve_fn(goal_exp, domain, negative, max_cost=min(store.best_cost, constraints.max_cost), unit_efforts=unit_efforts, max_effort=max_effort, effort_weight=effort_weight, **search_kwargs) if (max_skeletons is not None) and (len(skeleton_queue.skeletons) < max_skeletons): combined_plan, cost = iterative_plan_streams(evaluations, externals, optimistic_solve_fn, complexity_limit, unit_efforts=unit_efforts, max_effort=max_effort) else: combined_plan, cost = INFEASIBLE, INF if action_info: combined_plan = reorder_combined_plan(evaluations, combined_plan, full_action_info, domain) print('Combined plan: {}'.format(combined_plan)) stream_plan, action_plan = separate_plan(combined_plan, full_action_info) #stream_plan = replan_with_optimizers(evaluations, stream_plan, domain, externals) stream_plan = combine_optimizers(evaluations, stream_plan) #stream_plan = get_synthetic_stream_plan(stream_plan, # evaluations # [s for s in synthesizers if not s.post_only]) if reorder: stream_plan = reorder_stream_plan(stream_plan) # This may be redundant when using reorder_combined_plan print('Stream plan ({}, {:.3f}): {}\nAction plan ({}, {:.3f}): {}'.format( get_length(stream_plan), compute_plan_effort(stream_plan), stream_plan, get_length(action_plan), cost, str_from_plan(action_plan))) if is_plan(stream_plan) and visualize: log_plans(stream_plan, action_plan, num_iterations) create_visualizations(evaluations, stream_plan, num_iterations) search_time += elapsed_time(start_time) if (stream_plan is INFEASIBLE) and (not eager_instantiator) and (not skeleton_queue) and (not disabled): break start_time = time.time() if not is_plan(stream_plan): complexity_limit += complexity_step if not eager_disabled: reenable_disabled(evaluations, domain, disabled) elif not stream_plan: store.add_plan(action_plan, cost) if max_skeletons is None: process_stream_plan(store, domain, disabled, stream_plan) else: allocated_sample_time = (search_sample_ratio * search_time) - sample_time skeleton_queue.process(stream_plan, action_plan, cost, complexity_limit, allocated_sample_time) sample_time += elapsed_time(start_time) write_stream_statistics(externals + synthesizers, verbose) return store.extract_solution()
def collect_scoop(world): arm = LEFT_ARM spoon_name = create_name(random.choice(SCOOP_SPOONS), 1) bowl_name = create_name(random.choice(SCOOP_BOWLS), 1) item_ranges = { spoon_name: InitialRanges( width_range=(1., 1.), height_range=(1., 1.), mass_range=(1., 1.), # TODO: randomize density? pose2d_range=([0.3, 0.5, -np.pi / 2], [0.3, 0.5, -np.pi / 2]), ), bowl_name: InitialRanges( width_range=(0.8, 1.2), height_range=(0.8, 1.2), mass_range=(0.8, 1.2), pose2d_range=([0.5, 0.0, -np.pi], [0.5, 0.0, np.pi]), # x, y, theta ), } #dump_ranges(SCOOP_SPOONS, None) dump_ranges(SCOOP_BOWLS, item_ranges[bowl_name]) ################################################## # TODO: check collisions/feasibility when sampling #bowl_fraction = random.uniform(0.5, 0.75) bowl_fraction = random.uniform(0.75, 0.75) print('Bowl fraction:', bowl_fraction) #bead_radius = sample_norm(mu=0.005, sigma=0.0005, lower=0.004, upper=0.007) bead_radius = 0.005 print('Bead radius:', bead_radius) # Chickpeas have a 1cm diameter max_beads = 250 # TODO: could give the bowl infinite mass with ClientSaver(world.client): create_table_bodies(world, item_ranges) bowl_body = world.get_body(bowl_name) update_world(world, target_body=bowl_body) parameters_from_name = randomize_dynamics(world) parameters_from_name['bead'] = sample_bead_parameters() with LockRenderer(): all_beads = create_beads(max_beads, bead_radius, parameters=parameters_from_name['bead']) spoon_capacity = estimate_spoon_capacity(world, spoon_name, all_beads) print('{} | Capacity: {} | Mass: {:.3f}'.format( spoon_name, spoon_capacity, 0.0)) if spoon_capacity < MIN_CAPACITY: return INFEASIBLE init_holding = hold_item(world, arm, spoon_name) if init_holding is None: return INFEASIBLE # TODO: relate to the diameter of the spoon head. Ensure fraction above this level init_beads = fill_with_beads(world, bowl_name, all_beads, reset_contained=False, height_fraction=bowl_fraction) #wait_for_user() masses = list(map(get_mass, init_beads)) mean_mass = np.average(masses) init_mass = sum(masses) print('Init beads: {} | Init mass: {:.3f} | Mean mass: {:.3f}'.format( len(init_beads), init_mass, mean_mass)) if len(init_beads) < 2 * spoon_capacity: return INFEASIBLE world.initial_beads.update({bead: bowl_body for bead in init_beads}) init = [('Contains', bowl_name, COFFEE)] goal = [('Contains', spoon_name, COFFEE)] skeleton = [ ('move-arm', [arm, X, X, X]), ('scoop', [arm, bowl_name, X, spoon_name, X, COFFEE, X, X, X]), ] constraints = PlanConstraints(skeletons=[skeleton], exact=True) task = Task(init=init, goal=goal, arms=[arm], init_holding=init_holding, constraints=constraints) ################################################## feature = get_scoop_feature(world, bowl_name, spoon_name) initial_pose = world.get_pose(bowl_name) def score_fn(plan): assert plan is not None final_pose = world.get_pose(bowl_name) point_distance = get_distance(point_from_pose(initial_pose), point_from_pose(final_pose)) #, norm=2) quat_distance = quat_angle_between(quat_from_pose(initial_pose), quat_from_pose(final_pose)) print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format( point_distance, quat_distance)) with ClientSaver(world.client): bowl_beads = get_contained_beads(bowl_body, init_beads) fraction_bowl = float( len(bowl_beads)) / len(init_beads) if init_beads else 0 mass_in_bowl = sum(map(get_mass, bowl_beads)) spoon_beads = get_contained_beads(world.get_body(spoon_name), init_beads) fraction_spoon = float( len(spoon_beads)) / len(init_beads) if init_beads else 0 mass_in_spoon = sum(map(get_mass, spoon_beads)) print('In Bowl: {:.3f} | In Spoon: {:.3f}'.format( fraction_bowl, fraction_spoon)) # TODO: measure change in roll/pitch # TODO: could make latent parameters field score = { # Displacements 'bowl_translation': point_distance, 'bowl_rotation': quat_distance, # Masses 'total_mass': init_mass, 'mass_in_bowl': mass_in_bowl, 'mass_in_spoon': mass_in_spoon, 'spoon_mass_capacity': (init_mass / len(init_beads)) * spoon_capacity, # Counts 'num_beads': len(init_beads), 'bowl_beads': len(bowl_beads), 'spoon_beads': len(spoon_beads), 'spoon_capacity': spoon_capacity, # Fractions 'fraction_in_bowl': fraction_bowl, 'fraction_in_spoon': fraction_spoon, # Latent 'bead_radius': bead_radius, DYNAMICS: parameters_from_name } fraction_filled = float(score['spoon_beads']) / score['spoon_capacity'] spilled_beads = score['num_beads'] - (score['bowl_beads'] + score['spoon_beads']) fraction_spilled = float(spilled_beads) / score['num_beads'] print('Fraction Filled: {} | Fraction Spilled: {}'.format( fraction_filled, fraction_spilled)) #_, args = find_unique(lambda a: a[0] == 'scoop', plan) #control = args[-1] return score return task, feature, score_fn
def collect_push(world): arm = LEFT_ARM block_name = create_name('purpleblock', 1) item_ranges = { block_name: InitialRanges( width_range=(0.75, 1.25), height_range=(0.75, 1.25), mass_range=(1., 1.), pose2d_range=POSE2D_RANGE, # x, y, theta ), } ################################################## # TODO: check collisions/feasibility when sampling # TODO: grasps on the blue cup seem off for some reason... with ClientSaver(world.client): create_table_bodies(world, item_ranges) update_world(world, world.get_body(TABLE_NAME)) parameters_from_name = randomize_dynamics(world) stabilize(world) lower, upper = item_ranges[block_name].pose2d_range goal_pos2d = np.random.uniform(lower, upper)[:2] draw_push_goal(world, block_name, goal_pos2d) initial_pose = world.perception.get_pose(block_name) feature = get_push_feature(world, arm, block_name, initial_pose, goal_pos2d) init = [('CanPush', block_name, goal_pos2d)] goal = [('InRegion', block_name, goal_pos2d)] skeleton = [ ('move-arm', [arm, X, X, X]), ('push', [arm, block_name, X, X, X, X, X]), ] constraints = PlanConstraints(skeletons=[skeleton], exact=True) task = Task(init=init, goal=goal, arms=[arm], constraints=constraints) ################################################## def score_fn(plan): assert plan is not None initial_distance = get_distance( point_from_pose(initial_pose)[:2], goal_pos2d) final_pose = world.perception.get_pose(block_name) final_distance = get_distance( point_from_pose(final_pose)[:2], goal_pos2d) quat_distance = quat_angle_between(quat_from_pose(initial_pose), quat_from_pose(final_pose)) print( 'Initial: {:.5f} m | Final: {:.5f} | Rotation: {:.5f} rads'.format( initial_distance, final_distance, quat_distance)) # TODO: compare orientation to final predicted orientation # TODO: record simulation time in the event that the controller gets stuck score = { 'initial_distance': initial_distance, 'final_distance': final_distance, 'rotation': quat_distance, DYNAMICS: parameters_from_name, } #_, args = find_unique(lambda a: a[0] == 'push', plan) #control = args[-1] return score return task, feature, score_fn
def solve_abstract(problem, constraints=PlanConstraints(), stream_info={}, replan_actions=set(), unit_costs=False, success_cost=INF, max_time=INF, max_iterations=INF, max_memory=INF, initial_complexity=0, complexity_step=1, max_complexity=INF, max_skeletons=INF, search_sample_ratio=0, bind=True, max_failures=0, unit_efforts=False, max_effort=INF, effort_weight=None, reorder=True, visualize=False, verbose=True, **search_kwargs): """ Solves a PDDLStream problem by first planning with optimistic stream outputs and then querying streams :param problem: a PDDLStream problem :param constraints: PlanConstraints on the set of legal solutions :param stream_info: a dictionary from stream name to StreamInfo altering how individual streams are handled :param replan_actions: the actions declared to induce replanning for the purpose of deferred stream evaluation :param unit_costs: use unit action costs rather than numeric costs :param success_cost: the exclusive (strict) upper bound on plan cost to successfully terminate :param max_time: the maximum runtime :param max_iterations: the maximum number of search iterations :param max_memory: the maximum amount of memory :param initial_complexity: the initial stream complexity limit :param complexity_step: the increase in the stream complexity limit per iteration :param max_complexity: the maximum stream complexity limit :param max_skeletons: the maximum number of plan skeletons (max_skeletons=None indicates not adaptive) :param search_sample_ratio: the desired ratio of sample time / search time when max_skeletons!=None :param bind: if True, propagates parameter bindings when max_skeletons=None :param max_failures: the maximum number of stream failures before switching phases when max_skeletons=None :param unit_efforts: use unit stream efforts rather than estimated numeric efforts :param max_effort: the maximum amount of stream effort :param effort_weight: a multiplier for stream effort compared to action costs :param reorder: if True, reorder stream plans to minimize the expected sampling overhead :param visualize: if True, draw the constraint network and stream plan as a graphviz file :param verbose: if True, print the result of each stream application :param search_kwargs: keyword args for the search subroutine :return: a tuple (plan, cost, evaluations) where plan is a sequence of actions (or None), cost is the cost of the plan (INF if no plan), and evaluations is init expanded using stream applications """ # TODO: select whether to search or sample based on expected success rates # TODO: no optimizers during search with relaxed_stream_plan # TODO: locally optimize only after a solution is identified # TODO: replan with a better search algorithm after feasible # TODO: change the search algorithm and unit costs based on the best cost use_skeletons = (max_skeletons is not None) #assert implies(use_skeletons, search_sample_ratio > 0) eager_disabled = (effort_weight is None ) # No point if no stream effort biasing num_iterations = eager_calls = 0 complexity_limit = initial_complexity evaluations, goal_exp, domain, externals = parse_problem( problem, stream_info=stream_info, constraints=constraints, unit_costs=unit_costs, unit_efforts=unit_efforts) identify_non_producers(externals) enforce_simultaneous(domain, externals) compile_fluent_streams(domain, externals) # TODO: make effort_weight be a function of the current cost # if (effort_weight is None) and not has_costs(domain): # effort_weight = 1 load_stream_statistics(externals) if visualize and not has_pygraphviz(): visualize = False print( 'Warning, visualize=True requires pygraphviz. Setting visualize=False' ) if visualize: reset_visualizations() streams, functions, negative, optimizers = partition_externals( externals, verbose=verbose) eager_externals = list(filter(lambda e: e.info.eager, externals)) positive_externals = streams + functions + optimizers has_optimizers = bool(optimizers) # TODO: deprecate assert implies(has_optimizers, use_skeletons) ################ store = SolutionStore(evaluations, max_time, success_cost, verbose, max_memory=max_memory) skeleton_queue = SkeletonQueue(store, domain, disable=not has_optimizers) disabled = set() # Max skeletons after a solution while (not store.is_terminated()) and ( num_iterations < max_iterations) and (complexity_limit <= max_complexity): num_iterations += 1 eager_instantiator = Instantiator( eager_externals, evaluations) # Only update after an increase? if eager_disabled: push_disabled(eager_instantiator, disabled) if eager_externals: eager_calls += process_stream_queue( eager_instantiator, store, complexity_limit=complexity_limit, verbose=verbose) ################ print( '\nIteration: {} | Complexity: {} | Skeletons: {} | Skeleton Queue: {} | Disabled: {} | Evaluations: {} | ' 'Eager Calls: {} | Cost: {:.3f} | Search Time: {:.3f} | Sample Time: {:.3f} | Total Time: {:.3f}' .format(num_iterations, complexity_limit, len(skeleton_queue.skeletons), len(skeleton_queue), len(disabled), len(evaluations), eager_calls, store.best_cost, store.search_time, store.sample_time, store.elapsed_time())) optimistic_solve_fn = get_optimistic_solve_fn( goal_exp, domain, negative, replan_actions=replan_actions, reachieve=use_skeletons, max_cost=min(store.best_cost, constraints.max_cost), max_effort=max_effort, effort_weight=effort_weight, **search_kwargs) # TODO: just set unit effort for each stream beforehand if (max_skeletons is None) or (len(skeleton_queue.skeletons) < max_skeletons): disabled_axioms = create_disabled_axioms( skeleton_queue) if has_optimizers else [] if disabled_axioms: domain.axioms.extend(disabled_axioms) stream_plan, opt_plan, cost = iterative_plan_streams( evaluations, positive_externals, optimistic_solve_fn, complexity_limit, max_effort=max_effort) for axiom in disabled_axioms: domain.axioms.remove(axiom) else: stream_plan, opt_plan, cost = OptSolution( INFEASIBLE, INFEASIBLE, INF) # TODO: apply elsewhere ################ #stream_plan = replan_with_optimizers(evaluations, stream_plan, domain, externals) or stream_plan stream_plan = combine_optimizers(evaluations, stream_plan) #stream_plan = get_synthetic_stream_plan(stream_plan, # evaluations # [s for s in synthesizers if not s.post_only]) #stream_plan = recover_optimistic_outputs(stream_plan) if reorder: # TODO: this blows up memory wise for long stream plans stream_plan = reorder_stream_plan(store, stream_plan) num_optimistic = sum(r.optimistic for r in stream_plan) if stream_plan else 0 action_plan = opt_plan.action_plan if is_plan(opt_plan) else opt_plan print('Stream plan ({}, {}, {:.3f}): {}\nAction plan ({}, {:.3f}): {}'. format(get_length(stream_plan), num_optimistic, compute_plan_effort(stream_plan), stream_plan, get_length(action_plan), cost, str_from_plan(action_plan))) if is_plan(stream_plan) and visualize: log_plans(stream_plan, action_plan, num_iterations) create_visualizations(evaluations, stream_plan, num_iterations) ################ if (stream_plan is INFEASIBLE) and (not eager_instantiator) and ( not skeleton_queue) and (not disabled): break if not is_plan(stream_plan): print('No plan: increasing complexity from {} to {}'.format( complexity_limit, complexity_limit + complexity_step)) complexity_limit += complexity_step if not eager_disabled: reenable_disabled(evaluations, domain, disabled) #print(stream_plan_complexity(evaluations, stream_plan)) if not use_skeletons: process_stream_plan(store, domain, disabled, stream_plan, opt_plan, cost, bind=bind, max_failures=max_failures) continue ################ #optimizer_plan = replan_with_optimizers(evaluations, stream_plan, domain, optimizers) optimizer_plan = None if optimizer_plan is not None: # TODO: post process a bound plan print('Optimizer plan ({}, {:.3f}): {}'.format( get_length(optimizer_plan), compute_plan_effort(optimizer_plan), optimizer_plan)) skeleton_queue.new_skeleton(optimizer_plan, opt_plan, cost) allocated_sample_time = (search_sample_ratio * store.search_time) - store.sample_time \ if len(skeleton_queue.skeletons) <= max_skeletons else INF if skeleton_queue.process(stream_plan, opt_plan, cost, complexity_limit, allocated_sample_time) is INFEASIBLE: break ################ summary = store.export_summary() summary.update({ 'iterations': num_iterations, 'complexity': complexity_limit, 'skeletons': len(skeleton_queue.skeletons), }) print('Summary: {}'.format(str_from_object( summary, ndigits=3))) # TODO: return the summary write_stream_statistics(externals, verbose) return store.extract_solution()
def solve_focused(problem, constraints=PlanConstraints(), stream_info={}, replan_actions=set(), max_time=INF, max_iterations=INF, initial_complexity=0, complexity_step=1, max_skeletons=INF, bind=True, max_failures=0, unit_costs=False, success_cost=INF, unit_efforts=False, max_effort=INF, effort_weight=None, reorder=True, search_sample_ratio=0, visualize=False, verbose=True, **search_kwargs): """ Solves a PDDLStream problem by first hypothesizing stream outputs and then determining whether they exist :param problem: a PDDLStream problem :param constraints: PlanConstraints on the set of legal solutions :param stream_info: a dictionary from stream name to StreamInfo altering how individual streams are handled :param max_time: the maximum amount of time to apply streams :param max_iterations: the maximum number of search iterations :param max_skeletons: the maximum number of plan skeletons to consider :param unit_costs: use unit action costs rather than numeric costs :param success_cost: an exclusive (strict) upper bound on plan cost to terminate :param unit_efforts: use unit stream efforts rather than estimated numeric efforts :param initial_complexity: the initial effort limit :param complexity_step: the increase in the effort limit after each failure :param max_effort: the maximum amount of effort to consider for streams :param effort_weight: a multiplier for stream effort compared to action costs :param reorder: if True, stream plans are reordered to minimize the expected sampling overhead :param search_sample_ratio: the desired ratio of search time / sample time :param visualize: if True, it draws the constraint network and stream plan as a graphviz file :param verbose: if True, this prints the result of each stream application :param search_kwargs: keyword args for the search subroutine :return: a tuple (plan, cost, evaluations) where plan is a sequence of actions (or None), cost is the cost of the plan, and evaluations is init but expanded using stream applications """ # TODO: select whether to search or sample based on expected success rates # TODO: no optimizers during search with relaxed_stream_plan # TODO: locally optimize only after a solution is identified # TODO: replan with a better search algorithm after feasible num_iterations = search_time = sample_time = eager_calls = 0 complexity_limit = initial_complexity # TODO: make effort_weight be a function of the current cost # TODO: change the search algorithm and unit costs based on the best cost eager_disabled = effort_weight is None # No point if no stream effort biasing evaluations, goal_exp, domain, externals = parse_problem( problem, stream_info=stream_info, constraints=constraints, unit_costs=unit_costs, unit_efforts=unit_efforts) store = SolutionStore(evaluations, max_time, success_cost, verbose) load_stream_statistics(externals) if visualize and not has_pygraphviz(): visualize = False print( 'Warning, visualize=True requires pygraphviz. Setting visualize=False' ) if visualize: reset_visualizations() streams, functions, negative, optimizers = partition_externals( externals, verbose=verbose) eager_externals = list(filter(lambda e: e.info.eager, externals)) use_skeletons = max_skeletons is not None has_optimizers = bool(optimizers) assert implies(has_optimizers, use_skeletons) skeleton_queue = SkeletonQueue(store, domain, disable=not has_optimizers) disabled = set() # Max skeletons after a solution while (not store.is_terminated()) and (num_iterations < max_iterations): start_time = time.time() num_iterations += 1 eager_instantiator = Instantiator( eager_externals, evaluations) # Only update after an increase? if eager_disabled: push_disabled(eager_instantiator, disabled) eager_calls += process_stream_queue(eager_instantiator, store, complexity_limit=complexity_limit, verbose=verbose) print( '\nIteration: {} | Complexity: {} | Skeletons: {} | Skeleton Queue: {} | Disabled: {} | Evaluations: {} | ' 'Eager Calls: {} | Cost: {:.3f} | Search Time: {:.3f} | Sample Time: {:.3f} | Total Time: {:.3f}' .format(num_iterations, complexity_limit, len(skeleton_queue.skeletons), len(skeleton_queue), len(disabled), len(evaluations), eager_calls, store.best_cost, search_time, sample_time, store.elapsed_time())) optimistic_solve_fn = get_optimistic_solve_fn( goal_exp, domain, negative, replan_actions=replan_actions, reachieve=use_skeletons, max_cost=min(store.best_cost, constraints.max_cost), max_effort=max_effort, effort_weight=effort_weight, **search_kwargs) # TODO: just set unit effort for each stream beforehand if (max_skeletons is None) or (len(skeleton_queue.skeletons) < max_skeletons): disabled_axioms = create_disabled_axioms( skeleton_queue) if has_optimizers else [] if disabled_axioms: domain.axioms.extend(disabled_axioms) stream_plan, action_plan, cost = iterative_plan_streams( evaluations, (streams + functions + optimizers), optimistic_solve_fn, complexity_limit, max_effort=max_effort) for axiom in disabled_axioms: domain.axioms.remove(axiom) else: stream_plan, action_plan, cost = INFEASIBLE, INFEASIBLE, INF #stream_plan = replan_with_optimizers(evaluations, stream_plan, domain, externals) or stream_plan stream_plan = combine_optimizers(evaluations, stream_plan) #stream_plan = get_synthetic_stream_plan(stream_plan, # evaluations # [s for s in synthesizers if not s.post_only]) if reorder: stream_plan = reorder_stream_plan( stream_plan ) # This may be redundant when using reorder_combined_plan num_optimistic = sum(r.optimistic for r in stream_plan) if stream_plan else 0 print('Stream plan ({}, {}, {:.3f}): {}\nAction plan ({}, {:.3f}): {}'. format(get_length(stream_plan), num_optimistic, compute_plan_effort(stream_plan), stream_plan, get_length(action_plan), cost, str_from_plan(action_plan))) if is_plan(stream_plan) and visualize: log_plans(stream_plan, action_plan, num_iterations) create_visualizations(evaluations, stream_plan, num_iterations) search_time += elapsed_time(start_time) if (stream_plan is INFEASIBLE) and (not eager_instantiator) and ( not skeleton_queue) and (not disabled): break start_time = time.time() if not is_plan(stream_plan): complexity_limit += complexity_step if not eager_disabled: reenable_disabled(evaluations, disabled) #print(stream_plan_complexity(evaluations, stream_plan)) if use_skeletons: #optimizer_plan = replan_with_optimizers(evaluations, stream_plan, domain, optimizers) optimizer_plan = None if optimizer_plan is not None: # TODO: post process a bound plan print('Optimizer plan ({}, {:.3f}): {}'.format( get_length(optimizer_plan), compute_plan_effort(optimizer_plan), optimizer_plan)) skeleton_queue.new_skeleton(optimizer_plan, action_plan, cost) allocated_sample_time = (search_sample_ratio * search_time) - sample_time \ if len(skeleton_queue.skeletons) <= max_skeletons else INF skeleton_queue.process(stream_plan, action_plan, cost, complexity_limit, allocated_sample_time) else: process_stream_plan(store, domain, disabled, stream_plan, action_plan, cost, bind=bind, max_failures=max_failures) sample_time += elapsed_time(start_time) write_stream_statistics(externals, verbose) return store.extract_solution()