def create_dataset(self, include_invalid=False, include_none=True, **kwargs): # if (result.get(PARAMETER, None) is not None) return Dataset(self, [ result for result in self.results if implies(not result.get('valid', True), include_invalid) and implies(result.get(SCORE, None) is None, include_none) ], **kwargs)
def pddlstream_from_tamp(tamp_problem, use_stream=True, use_optimizer=False, collisions=True): initial = tamp_problem.initial assert(initial.holding is None) domain_pddl = read(get_file_path(__file__, 'domain.pddl')) external_paths = [] if use_stream: external_paths.append(get_file_path(__file__, 'stream.pddl')) if use_optimizer: external_paths.append(get_file_path(__file__, 'optimizer.pddl')) external_pddl = [read(path) for path in external_paths] constant_map = {} init = [ ('CanMove',), ('Conf', initial.conf), ('AtConf', initial.conf), ('HandEmpty',), Equal((TOTAL_COST,), 0)] + \ [('Block', b) for b in initial.block_poses.keys()] + \ [('Pose', b, p) for b, p in initial.block_poses.items()] + \ [('AtPose', b, p) for b, p in initial.block_poses.items()] + \ [('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] + \ [('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()] + \ [('Region', r) for r in tamp_problem.goal_regions.values() + [GROUND_NAME]] goal_literals = [('In', b, r) for b, r in tamp_problem.goal_regions.items()] #+ [('HandEmpty',)] if tamp_problem.goal_conf is not None: goal_literals += [('AtConf', tamp_problem.goal_conf)] goal = And(*goal_literals) stream_map = { 's-motion': from_fn(plan_motion), 's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)), 't-region': from_test(get_region_test(tamp_problem.regions)), 's-ik': from_fn(inverse_kin_fn), #'s-ik': from_gen_fn(unreliable_ik_fn), 'distance': distance_fn, 't-cfree': from_test(lambda *args: implies(collisions, not collision_test(*args))), #'posecollision': collision_test, # Redundant 'trajcollision': lambda *args: False, } if use_optimizer: stream_map.update({ 'gurobi': from_fn(get_optimize_fn(tamp_problem.regions)), 'rrt': from_fn(cfree_motion_fn), }) #stream_map = 'debug' return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map, init, goal)
def pddlstream_from_tamp(tamp_problem, use_stream=True, use_optimizer=False, collisions=True): domain_pddl = read(get_file_path(__file__, 'domain.pddl')) external_paths = [] if use_stream: external_paths.append(get_file_path(__file__, 'stream.pddl')) if use_optimizer: external_paths.append( get_file_path( __file__, 'optimizer/optimizer.pddl')) # optimizer | optimizer_hard external_pddl = [read(path) for path in external_paths] constant_map = {} stream_map = { 's-grasp': from_fn(lambda b: (GRASP, )), 's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)), 's-ik': from_fn(inverse_kin_fn), #'s-ik': from_gen_fn(unreliable_ik_fn), 's-motion': from_fn(plan_motion), 't-region': from_test(get_region_test(tamp_problem.regions)), 't-cfree': from_test( lambda *args: implies(collisions, not collision_test(*args))), 'dist': distance_fn, 'duration': duration_fn, } if use_optimizer: # To avoid loading gurobi stream_map.update({ 'gurobi': from_list_fn( get_optimize_fn(tamp_problem.regions, collisions=collisions)), 'rrt': from_fn(cfree_motion_fn), }) #stream_map = 'debug' init, goal = create_problem(tamp_problem) return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map, init, goal)
def main(): # collect data in parallel, parameters are generated uniformly randomly in a range # data stored to pour_date/trails_n=10000.json # TODO: ulimit settings # https://ss64.com/bash/ulimit.html # https://stackoverflow.com/questions/938733/total-memory-used-by-python-process # import psutil # TODO: resource.getrusage(resource.RUSAGE_SELF).ru_maxrss assert (get_python_version() == 3) parser = argparse.ArgumentParser() parser.add_argument('-f', '--fn', default=TRAINING, help='The parameter function to use.') parser.add_argument('-n', '--num', type=int, default=10000, help='The number of samples to collect.') parser.add_argument('-p', '--problem', required=True, choices=sorted(SKILL_COLLECTORS.keys()), help='The name of the skill to learn.') parser.add_argument('-t', '--time', type=int, default=1 * 60, help='The max planning runtime for each trial.') parser.add_argument('-v', '--visualize', action='store_true', help='When enabled, visualizes execution.') args = parser.parse_args() serial = is_darwin() assert implies(args.visualize, serial) trials = get_trials(args.problem, args.fn, args.num, max_time=args.time, valid=True, visualize=args.visualize, verbose=serial) data_path = None if serial else get_data_path(args.problem, trials) num_cores = get_num_cores(trials, serial) user_input('Begin?') # TODO: store the generating distribution for samples and objects? print(SEPARATOR) results = run_trials(trials, data_path, num_cores=num_cores)
def optimistic_stream_instantiation(instance, bindings, evaluations, opt_evaluations, only_immediate=False): # TODO: combination for domain predicates new_instances = [] for input_combo in product(*[bindings.get(i, [i]) for i in instance.input_objects]): mapping = get_mapping(instance.input_objects, input_combo) domain_evaluations = set(map(evaluation_from_fact, substitute_expression( instance.get_domain(), mapping))) # TODO: could just instantiate first if domain_evaluations <= opt_evaluations: new_instance = instance.external.get_instance(input_combo) # TODO: method for eagerly evaluating some of these? if (new_instance.opt_index != 0) and implies(only_immediate, domain_evaluations <= evaluations): new_instance.opt_index -= 1 new_instances.append(new_instance) return new_instances
def separate_plan(combined_plan, action_info=None, terminate=False, stream_only=True): if not is_plan(combined_plan): return combined_plan, combined_plan stream_plan = [] action_plan = [] terminated = False for operator in combined_plan: if terminate and terminated: break if isinstance(operator, Result): if terminated: if implies(stream_only, isinstance(operator, StreamResult)): action_plan.append(operator.get_tuple()) else: stream_plan.append(operator) else: action_plan.append(operator) if action_info is not None: name = operator[0] terminated |= action_info[name].terminal return stream_plan, action_plan
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 main(): """ ./home/demo/catkin_percep/collect_scales.sh (includes scale offset) Make sure to start the scales with nothing on them (for calibration) """ assert (get_python_version() == 2) # ariadne has ROS with python2 parser = argparse.ArgumentParser() parser.add_argument('-a', '--active', action='store_true', help='Uses active learning queries.') parser.add_argument('-d', '--debug', action='store_true', help='Disables saving during debugging.') parser.add_argument( '-f', '--fn', type=str, default=TRAINING, # DESIGNED | TRAINING help='The name of or the path to the policy that generates parameters.' ) parser.add_argument('-m', '--material', required=True, choices=sorted(MATERIALS), help='The name of the material being used.') parser.add_argument('-p', '--problem', required=True, choices=sorted(REQUIREMENT_FNS.keys()), help='The name of the skill to learn.') parser.add_argument('-s', '--spoon', default=None, choices=SPOONS, help='The name of the spoon being used.') parser.add_argument('-r', '--train', action='store_true', help='When enabled, uses the training dataset.') parser.add_argument( '-v', '--visualize_planning', action='store_true', help= 'When enabled, visualizes planning rather than the world (for debugging).' ) args = parser.parse_args() # TODO: toggle material default based on task # TODO: label material on the image assert args.material in MODEL_MASSES print('Policy:', args.fn) assert implies(args.problem in ['scoop'], args.spoon is not None) assert implies(args.active, args.train) ros_world = ROSWorld(sim_only=False, visualize=not args.visualize_planning) classes_pub = rospy.Publisher('~collect_classes', String, queue_size=1) with ros_world: set_camera_pose(np.array([1.5, -0.5, 1.5]), target_point=np.array([0.75, 0, 0.75])) arm, is_open = ACTIVE_ARMS[args.problem] open_grippers = {arm: is_open} if args.problem == 'scoop': ros_world.controller.open_gripper(get_arm_prefix(arm), blocking=True) ros_world.controller.speak("{:.0f} seconds to attach {}".format( ATTACH_TIME, format_class(args.spoon))) rospy.sleep(ATTACH_TIME) # Sleep to have time to set the spoon move_to_initial_config(ros_world, open_grippers) #get_other_arm # TODO: cross validation for measuring performance across a few bowls launch = launch_kinect() video_time = time.time() if args.debug: ros_world.controller.speak("Warning! Data will not be saved.") time.sleep(1.0) data_path = None else: # TODO: only create directory if examples made data_path = get_data_path(args.problem, real=True) # TODO: log camera image after the pour policy = args.fn learner_path = None test_data = None if isinstance(policy, str) and os.path.isfile(policy): policy = read_pickle(policy) assert isinstance(policy, ActiveLearner) print(policy) print(policy.algorithm) #policy.transfer_weight = 0 # print(policy.xx.shape) # policy.results = policy.results[:-1] # policy.xx = policy.xx[:-1] # policy.yy = policy.yy[:-1] # policy.weights = policy.weights[:-1] # print(policy.xx.shape) # write_pickle(args.fn, policy) # print('Saved', args.fn) if args.active: # policy.retrain() test_domain = load_data(SCOOP_TEST_DATASETS, verbose=False) test_data = test_domain.create_dataset(include_none=False, binary=False) policy.query_type = STRADDLE # VARIANCE #policy.weights = 0.1*np.ones(policy.yy.shape) # TODO: make this multiplicative #policy.retrain() evaluate_confusions(test_data, policy) else: policy.query_type = BEST ensure_dir(LEARNER_DIRECTORY) date_name = datetime.datetime.now().strftime(DATE_FORMAT) filename = '{}_{}.pk{}'.format(get_label(policy.algorithm), date_name, get_python_version()) learner_path = os.path.join(LEARNER_DIRECTORY, filename) if ACTIVE_FEATURE and args.active: assert isinstance(policy, ActiveLearner) generator = create_active_generator(args, policy) else: generator = create_random_generator(args) pair = next(generator) print('Next pair:', pair) classes_pub.publish('{},{}'.format(*pair)) for phrase in map(format_class, pair): ros_world.controller.speak(phrase) wait_for_user('Press enter to begin') # TODO: change the name of the directory after additional samples results = [] num_trials = num_failures = num_scored = 0 while True: start_time = elapsed_time(video_time) result = run_loop(args, ros_world, policy) print('Result:', str_from_object(result)) print('{}\nTrials: {} | Successes: {} | Failures: {} | Time: {:.3f}'. format(SEPARATOR, num_trials, len(results), num_failures, elapsed_time(video_time))) num_trials += 1 if result is None: # TODO: result['execution'] num_failures += 1 print('Error! Trial resulted in an exception') move_to_initial_config(ros_world, open_grippers) continue end_time = elapsed_time(video_time) print('Elapsed time:', end_time - start_time) # TODO: record the type of failure (planning, execution, etc...) scored = result['score'] is not None num_scored += scored # TODO: print the score if isinstance(policy, ActiveLearner) and args.active: # and scored: update_learner(policy, learner_path, result) evaluate_confusions(test_data, policy) # TODO: how to handle failures that require bad annotations? pair = next(generator) print('Next pair:', pair) classes_pub.publish('{},{}'.format(*pair)) for phrase in map(format_class, pair): ros_world.controller.speak(phrase) annotation = wait_for_user( 'Enter annotation and press enter to continue: ') result.update({ # TODO: record the query_type 'policy': args.fn, 'active_feature': ACTIVE_FEATURE, 'trial': num_trials, 'start_time': start_time, 'end_time': end_time, 'annotation': annotation, }) results.append(result) if data_path is not None: write_results(data_path, results) #if annotation in ['q', 'quit']: # TODO: Ctrl-C to quit # break ros_world.controller.speak("Finished") if launch is not None: launch.shutdown() print('Total time:', elapsed_time(video_time))
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 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('experiments', nargs='+', help='Name of the experiment') args = parser.parse_args() outcomes_per_task = {} for path in args.experiments: for result in read_json(path): experiment = result['experiment'] problem = experiment['problem'] outcome = result['outcome'] #policy = frozenset(experiment['policy'].items()) policy = name_from_policy(experiment['policy']) outcomes_per_task.setdefault(problem['task'], {}).setdefault(policy, []).append(outcome) #outcomes_per_task['inspect_drawer']['constrain=0_defer=1'].append(ERROR_OUTCOME) #outcomes_per_task['detect_block']['constrain=1_defer=0'].append(ERROR_OUTCOME) # TODO: robust poses # TODO: intelligent IR for pour table = '' for task in TASK_NAMES: if task not in outcomes_per_task: continue print('\nTask: {}'.format(task)) items = [task] for policy in POLICIES: policy = name_from_policy(policy) if policy not in outcomes_per_task[task]: continue outcomes = list(take(outcomes_per_task[task][policy], MAX_TRIALS)) value_per_attribute = {} for outcome in outcomes: if outcome['error']: outcome.update(ERROR_OUTCOME) if MAX_TIME < outcome.get('total_time', INF): outcome['achieved_goal'] = False if not outcome['achieved_goal']: outcome['total_time'] = MAX_TIME outcome['plan_time'] = MAX_TIME for attribute, value in outcome.items(): if (attribute not in ['policy']) and (attribute in PRINT_ATTRIBUTES) and \ not isinstance(value, str) and implies(attribute in ACHIEVED_GOAL, outcome['achieved_goal']): value_per_attribute.setdefault(attribute, []).append(float(value)) statistics = { attribute: np.round(np.average(values), 3) # '{:.2f}'.format( for attribute, values in value_per_attribute.items() } # median, min, max of solved? statistics['trials'] = len(outcomes) print('{}: {}'.format(policy, str_from_object(statistics))) items += [ '{:.0f}'.format(100 * statistics['achieved_goal']), '{:.0f}'.format(statistics['plan_time']), ] table += '{}\n\\\\ \hline\n'.format(' & '.join(items)) print(SEPARATOR) print(POLICIES) print(table)
def process_stream_plan(evaluations, stream_plan, disabled, verbose, quick_fail=True, layers=False, max_values=INF): # TODO: can also use the instantiator and operate directly on the outputs # TODO: could bind by just using new_evaluations plan_index = get_stream_plan_index(stream_plan) streams_from_output = defaultdict(list) for result in stream_plan: if isinstance(result, StreamResult): for obj in result.output_objects: streams_from_output[obj].append(result) shared_output_streams = { s for streams in streams_from_output.values() if 1 < len(streams) for s in streams } #shared_output_streams = {} print(shared_output_streams) print(plan_index) opt_bindings = defaultdict(list) opt_evaluations = set() opt_results = [] failed = False stream_queue = deque(stream_plan) while stream_queue and implies(quick_fail, not failed): opt_result = stream_queue.popleft() real_instances, opt_instances = ground_stream_instances( opt_result.instance, opt_bindings, evaluations, opt_evaluations, plan_index) first_step = all( isinstance(o, Object) for o in opt_result.instance.input_objects) num_instances = min(len(real_instances), max_values) \ if (layers or first_step or (opt_result not in shared_output_streams)) else 0 opt_instances += real_instances[num_instances:] real_instances = real_instances[:num_instances] new_results = [] local_failure = False for instance in real_instances: results = instance.next_results(verbose=verbose) for result in results: add_certified(evaluations, result) disable_stream_instance(instance, disabled) local_failure |= not results if isinstance(opt_result, PredicateResult) and not any( opt_result.value == r.value for r in results): local_failure = True # TODO: check for instance? new_results += results for instance in opt_instances: #print(instance, instance.opt_index) results = instance.next_optimistic() opt_evaluations.update( evaluation_from_fact(f) for r in results for f in r.get_certified()) opt_results += results local_failure |= not results new_results += results for result in new_results: if isinstance(result, StreamResult): # Could not add if same value for opt, obj in zip(opt_result.output_objects, result.output_objects): opt_bindings[opt].append(obj) if local_failure and isinstance(opt_result, SynthStreamResult): stream_queue.extendleft(reversed(opt_result.decompose())) failed = False # TODO: check if satisfies target certified else: failed |= local_failure if verbose: print('Success: {}'.format(not failed)) if failed: return None, None # TODO: just return binding # TODO: could also immediately switch to binding if plan_index == 0 afterwards return opt_results, opt_bindings
def pddlstream_from_tamp(tamp_problem, use_stream=True, use_optimizer=False, collisions=True): initial = tamp_problem.initial assert (not initial.holding) domain_pddl = read(get_file_path(__file__, 'domain.pddl')) external_paths = [] if use_stream: external_paths.append(get_file_path(__file__, 'stream.pddl')) if use_optimizer: external_paths.append(get_file_path( __file__, 'optimizer.pddl')) # optimizer | optimizer_hard external_pddl = [read(path) for path in external_paths] constant_map = {} init = [ ('Region', GROUND_NAME), Equal((TOTAL_COST,), 0)] + \ [('Block', b) for b in initial.block_poses.keys()] + \ [('Pose', b, p) for b, p in initial.block_poses.items()] + \ [('Grasp', b, GRASP) for b in initial.block_poses] + \ [('AtPose', b, p) for b, p in initial.block_poses.items()] + \ [('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] goal_literals = [] for r, q in initial.robot_confs.items(): init += [ ('Robot', r), ('CanMove', r), ('Conf', q), ('AtConf', r, q), ('HandEmpty', r), ] if tamp_problem.goal_conf is not None: #goal_literals += [('AtConf', tamp_problem.goal_conf)] goal_literals += [('AtConf', r, q)] for b, r in tamp_problem.goal_regions.items(): if isinstance(r, str): init += [('Region', r), ('Placeable', b, r)] goal_literals += [('In', b, r)] else: init += [('Pose', b, r)] goal_literals += [('AtPose', b, r)] #goal_literals += [Not(('Unsafe',))] # ('HandEmpty',) goal = And(*goal_literals) stream_map = { 's-motion': from_fn(plan_motion), 's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)), 't-region': from_test(get_region_test(tamp_problem.regions)), 's-ik': from_fn(inverse_kin_fn), #'s-ik': from_gen_fn(unreliable_ik_fn), 'dist': distance_fn, 'duration': duration_fn, 't-cfree': from_test( lambda *args: implies(collisions, not collision_test(*args))), } if use_optimizer: # To avoid loading gurobi stream_map.update({ 'gurobi': from_list_fn( get_optimize_fn(tamp_problem.regions, collisions=collisions)), 'rrt': from_fn(cfree_motion_fn), }) #stream_map = 'debug' return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map, init, goal)
def main(): parser = argparse.ArgumentParser() parser.add_argument('paths', nargs='*', help='Paths to the data.') #parser.add_argument('-a', '--active', type=int, default=0, # None # help='The number of active samples to collect') parser.add_argument( '-d', '--deterministic', action='store_true', help='Whether to deterministically create training splits') parser.add_argument('-n', '--num_trials', type=int, default=-1, help='The number of samples to collect') parser.add_argument('-s', '--save', action='store_true', help='Whether to save the learners') parser.add_argument('-r', '--num_rounds', type=int, default=1, help='The number of rounds to collect') parser.add_argument('-t', '--test', action='store_true', help='Whether to save the data') parser.add_argument('-v', '--visualize', action='store_true', help='When enabled, visualizes execution.') args = parser.parse_args() # TODO: be careful that paging isn't altering the data # TODO: use a different set of randomized parameters for train and test serial = is_darwin() visualize = serial and args.visualize assert implies(visualize, serial) num_trials = get_max_cores( serial) if args.num_trials < 0 else args.num_trials ################################################## #train_sizes = inclusive_range(50, 200, 10) # Best #train_sizes = inclusive_range(50, 400, 10) # F1 #train_sizes = inclusive_range(25, 400, 25) #train_sizes = inclusive_range(50, 100, 5) # Real #train_sizes = inclusive_range(100, 200, 5) #train_sizes = inclusive_range(10, 250, 5) #train_sizes = inclusive_range(35, 70, 5) #train_sizes = inclusive_range(5, 50, 5) #train_sizes = inclusive_range(40, 80, 5) #train_sizes = inclusive_range(100, 1000, 100) #train_sizes = [50] #train_sizes = [250] train_sizes = [1000] #train_sizes = [327] # train + test #train_sizes = inclusive_range(5, 150, 25) #train_sizes = [100] #kernels = ['RBF', 'Matern52', 'MLP'] kernels = ['MLP'] hyperparams = [None] #hyperparams = [True] #hyperparams = [None, True] query_type = BEST # BEST | CONFIDENT | REJECTION | ACTIVE # type of query used to evaluate the learner include_none = False binary = False # 0 => no transfer # 1 => mean transfer # 2 => kernel transfer # 3 => both transfer transfer_weights = [None] #transfer_weights = list(range(4)) #transfer_weights = [0, 1] #transfer_weights = [3] #transfer_weights = np.around(np.linspace(0.0, 1.0, num=1+5, endpoint=True), decimals=3) # max 10 colors #transfer_weights = list(range(1, 1+3)) #split = UNIFORM # BALANCED #print('Split:', split) #parameters = { # 'include None': include_none, # 'binary': binary, # 'split': split, #} # Omitting failed labels is okay because they will never be executed algorithms = [] #algorithms += [(Algorithm(nn_model, label='NN'), [num]) # for nn_model, num in product(NN_MODELS, train_sizes)] #algorithms += [(Algorithm(RANDOM), None), (Algorithm(DESIGNED), None)] #algorithms += [(Algorithm(RF_CLASSIFIER, variance=False, transfer_weight=tw, label='RF'), [num]) # for num, tw in product(train_sizes, [None])] # transfer_weights #algorithms += [(Algorithm(RF_REGRESSOR, variance=False, transfer_weight=tw, label='RF'), [num]) # for num, tw in product(train_sizes, [None])] # transfer_weights #algorithms += [(Algorithm(BATCH_RF, variance=True, transfer_weight=tw, label='RF'), [num]) # for num, tw in product(train_sizes, [None])] # transfer_weights #algorithms += [(Algorithm(BATCH_MAXVAR_RF, variance=True, transfer_weight=tw), train_sizes) # for tw in product(use_vars, [None])] # transfer_weights #algorithms += [(Algorithm(BATCH_STRADDLE_RF, variance=True, transfer_weight=tw), train_sizes) # for tw, in product([None])] # transfer_weights use_vars = [True] # STRADDLE is better than MAXVAR when the learner has a good estimate of uncertainty algorithms += [ (Algorithm(BATCH_GP, kernel, hype, use_var, tw, label='GP'), [num]) # label='GP-{}'.format(kernel) for num, kernel, hype, use_var, tw in product( train_sizes, kernels, hyperparams, use_vars, transfer_weights) ] #algorithms += [(Algorithm(BATCH_MAXVAR_GP, kernel, hype, True, tw, label='GP-Var'), train_sizes) # for kernel, hype, tw in product(kernels, hyperparams, transfer_weights)] #algorithms += [(Algorithm(BATCH_STRADDLE_GP, kernel, hype, True, tw, label='GP-LSE'), train_sizes) # for kernel, hype, tw in product(kernels, hyperparams, transfer_weights)] # default active #algorithms += [(Algorithm(BATCH_STRADDLE_GP, kernel, hype, True, tw, label='GP-LSE2'), train_sizes) # for kernel, hype, tw in product(kernels, hyperparams, transfer_weights)] # active control only # algorithms += [(Algorithm(MAXVAR_GP, kernel, hype, use_var), train_sizes) # for kernel, hype, use_var in product(kernels, hyperparams, use_vars)] #algorithms += [(Algorithm(STRADDLE_GP, kernel, hype, use_var, tw), train_sizes) # for kernel, hype, use_var, tw in product(kernels, hyperparams, use_vars, transfer_weights)] #batch_sizes = inclusive_range(train_sizes[0], 90, 10) #step_size = 10 # TODO: extract from train_sizes #final_size = train_sizes[-1] # Previously didn't have use_var=True # algorithms += [(Algorithm(BATCH_STRADDLE_GP, kernel, hyperparameters=batch_size, variance=True, transfer_weight=tw), # inclusive_range(batch_size, final_size, step_size)) # for kernel, tw, batch_size in product(kernels, transfer_weights, batch_sizes)] # algorithms += [(Algorithm(BATCH_STRADDLE_RF, hyperparameters=batch_size, variance=True, transfer_weight=tw), # inclusive_range(batch_size, final_size, step_size)) # for tw, batch_size in product(transfer_weights, batch_sizes)] print('Algorithms:', algorithms) ################################################## real_world = not args.paths transfer_domain = load_data(TRANSFER_DATASETS, verbose=False) transfer_algorithm = None if real_world and transfer_weights != [None]: #assert transfer_weights[0] is not None transfer_data = transfer_domain.create_dataset( include_none=include_none, binary=binary) transfer_algorithm = Algorithm(BATCH_GP, kernel=kernels[0], variance=use_vars[0]) validity_learner = None #validity_learner = create_validity_classifier(transfer_domain) ################################################## train_paths = args.paths if real_world: train_paths = SCOOP_TRAIN_DATASETS # TRAIN_DATASETS #train_paths = TRANSFER_DATASETS #train_paths = TRAIN_DATASETS + TRANSFER_DATASETS # Train before transfer #scale_paths = TRAIN_DATASETS + TEST_DATASETS scale_paths = None print(SEPARATOR) print('Train paths:', train_paths) domain = load_data(train_paths) print() print(domain) all_data = domain.create_dataset(include_none=include_none, binary=binary, scale_paths=scale_paths) #all_data.results = all_data.results[:1000] num_failed = 0 #num_failed = 100 failed_domain = transfer_domain if real_world else domain failed_results = randomize( result for result in failed_domain.results if not result.get('success', False))[:num_failed] #failed_data = Dataset(domain, failed_results, **all_data.kwargs) test_paths = SCOOP_TEST_DATASETS # TEST_DATASETS | SCOOP_TEST_DATASETS #test_paths = None if real_world and not (set(train_paths) & set(test_paths)): #assert not set(train_paths) & set(test_paths) #max_test = 0 test_data = load_data(test_paths).create_dataset( include_none=False, binary=binary, scale_paths=scale_paths) else: #assert scale_paths is None # TODO: max_train will be too small otherwise test_paths = test_data = None print(SEPARATOR) print('Test paths:', test_paths) all_active_data = None #if real_world: # all_active_data = load_data(ACTIVE_DATASETS).create_dataset(include_none=True, binary=binary, scale_paths=scale_paths) # TODO: could include OS and username if desired date_name = datetime.datetime.now().strftime(DATE_FORMAT) size_str = '[{},{}]'.format(train_sizes[0], train_sizes[-1]) #size_str = '-'.join(map(str, train_sizes)) experiments_name = '{}_r={}_t={}_n={}'.format(date_name, args.num_rounds, size_str, num_trials) trials_per_round = sum( 1 if train_sizes is None else (train_sizes[-1] - train_sizes[0] + len(train_sizes)) for _, train_sizes in algorithms) num_experiments = args.num_rounds * trials_per_round max_train = min( max([0] + [ active_sizes[0] for _, active_sizes in algorithms if active_sizes is not None ]), len(all_data)) max_test = min(len(all_data) - max_train, 1000) ################################################## # #features = ['bowl_height'] # features = ['spoon_height'] # #features = ['bowl_height', 'spoon_height'] # X, Y, _ = all_data.get_data() # #indices = [domain.inputs.index(feature) for feature in features] # #X = X[:,indices] # X = [[result[FEATURE][name] for name in features] for result in all_data.results] # from sklearn.linear_model import LinearRegression # model = LinearRegression(fit_intercept=True, normalize=False) # model.fit(X, Y) # #print(model.get_params()) # print(model.coef_.tolist(), model.intercept_) # print(model.score(X, Y)) #data_dir = os.path.join(DATA_DIRECTORY, domain.name) # EXPERIMENT_DIRECTORY data_dir = os.path.abspath(os.path.join(domain.name, os.path.pardir)) experiments_dir, data_path = None, None if not args.test or not serial: experiments_dir = os.path.join(data_dir, experiments_name) data_path = os.path.join( experiments_dir, 'experiments.pk{}'.format(get_python_version())) ################################################## print(SEPARATOR) print('Name:', experiments_name) print('Experiments:', num_experiments) print('Experiment dir:', experiments_dir) print('Data path:', data_path) print('Examples:', len(all_data)) print('Valid:', sum(result.get('valid', True) for result in all_data.results)) print('Success:', sum(result.get('success', False) for result in all_data.results)) print( 'Scored:', sum( result.get('score', None) is not None for result in all_data.results)) print('Max train:', max_train) print('Max test:', max_test) print('Include None:', include_none) print('Examples: n={}, d={}'.format(len(all_data), domain.dx)) print('Binary:', binary) print('Serial:', serial) print('Estimated hours: {:.3f}'.format(num_experiments * SEC_PER_EXPERIMENT / HOURS_TO_SECS)) user_input('Begin?') ################################################## experiments = [] if experiments_dir is not None: mkdir(experiments_dir) # if os.path.exists(data_path): # experiments.extend(read_pickle(data_path)) # TODO: embed in a KeyboardInterrupt to allow early termination start_time = time.time() for round_idx in range(args.num_rounds): seed = round_idx if args.deterministic else hash( time.time()) # vs just time.time()? random.seed(seed) all_data.shuffle() if test_paths is None: # cannot use test_data #test_data, train_data = split_data(all_data, max_test) train_data = test_data = all_data # Training performance else: train_data = all_data transfer_learner = None if transfer_algorithm is not None: round_data, _ = transfer_data.partition(index=1000) transfer_learner, _ = create_learner(transfer_domain, round_data, transfer_algorithm, verbose=True) transfer_learner.retrain() print(SEPARATOR) print('Round {} | Train examples: {} | Test examples: {}'.format( round_idx, len(train_data), len(test_data))) for algorithm, active_sizes in algorithms: # active_sizes = [first #trainingdata selected from X_train, #active exploration + #trainingdata] print(SEPARATOR) print('Round: {} | {} | Seed: {} | Sizes: {}'.format( round_idx, algorithm, seed, active_sizes)) # TODO: allow keyboard interrupt if active_sizes is None: learner = algorithm.name active_size = train_confusion = None experiments.append( evaluate_learner(domain, seed, train_confusion, test_data, algorithm, learner, active_size, num_trials, serial, args.visualize)) continue # [10 20 25] take first 10 samples from X_train to train the model, 10 samples chosen actively # sequentially + evaluate model, 5 samples chosen actively sequentially + evaluate model # Could always keep around all the examples and retrain # TODO: segfaults when this runs in parallel # TODO: may be able to retrain in parallel if I set OPENBLAS_NUM_THREADS num_batch = active_sizes[0] batch_data, active_data = train_data.partition(num_batch) if all_active_data is not None: active_data = all_active_data.clone() #batch_data.results.extend(failed_results) learner, train_confusion = create_learner( domain, batch_data, algorithm, # alphas, query_type=query_type, verbose=True) learner.validity_learner = validity_learner if transfer_learner is not None: learner.sim_model = transfer_learner.model learner.retrain() for active_size in active_sizes: num_active = active_size - (learner.nx - len(failed_results)) print('\nRound: {} | {} | Seed: {} | Size: {} | Active: {}'. format(round_idx, algorithm, seed, active_size, num_active)) if algorithm.name in CONTINUOUS_ACTIVE_GP: active_learning(learner, num_active, visualize=visualize) #active_learning(learner, num_active, discrete_feature=True, random_feature=False) #active_learning_discrete(learner, active_data, num_active, random_feature=False) elif algorithm.name in BATCH_ACTIVE: active_learning_discrete(learner, active_data, num_active) #active_learning(learner, num_active, discrete_feature=True, random_feature=True) #active_learning_discrete(learner, active_data, num_active, random_feature=True) #if round_dir is not None: # save_learner(round_dir, learner) if args.save: learner.save(data_dir) experiments.append( evaluate_learner(domain, seed, train_confusion, test_data, algorithm, learner, active_size, num_trials, serial, args.visualize)) save_experiments(data_path, experiments) print(SEPARATOR) if experiments: save_experiments(data_path, experiments) plot_experiments(domain, experiments_name, experiments_dir, experiments, include_none=False) print('Experiments: {}'.format(experiments_dir)) print('Total experiments: {}'.format(len(experiments))) print('Total hours: {:.3f}'.format( elapsed_time(start_time) / HOURS_TO_SECS))
def pdddlstream_from_problem(belief, additional_init=[], fixed_base=True, **kwargs): world = belief.world # One world per state task = world.task # One task per world print(task) domain_pddl = read(get_file_path(__file__, '../pddl/domain.pddl')) # TODO: repackage stream outputs to avoid recomputation # Despite the base not moving, it could be re-estimated init_bq = belief.base_conf init_aq = belief.arm_conf init_gq = belief.gripper_conf carry_aq = world.carry_conf init_aq = carry_aq if are_confs_close(init_aq, carry_aq) else init_aq # TODO: the following doesn't work. Maybe because carry_conf is used elsewhere #carry_aq = init_aq if are_confs_close(init_aq, world.carry_conf) else world.carry_conf #calibrate_aq = init_aq if are_confs_close(init_aq, world.calibrate_conf) else world.calibrate_conf # Don't need this now that returning to old confs #open_gq = init_gq if are_confs_close(init_gq, world.open_gq) else world.open_gq #closed_gq = init_gq if are_confs_close(init_gq, world.closed_gq) else world.closed_gq open_gq = world.open_gq closed_gq = world.closed_gq constant_map = { '@world': 'world', '@gripper': 'gripper', '@stove': 'stove', '@none': None, '@rest_aq': carry_aq, #'@calibrate_aq': calibrate_aq, '@open_gq': open_gq, '@closed_gq': closed_gq, '@open': OPEN, '@closed': CLOSED, '@top': TOP_GRASP, '@side': SIDE_GRASP, '@bq0': init_bq, } top_joint = JOINT_TEMPLATE.format(TOP_DRAWER) bottom_joint = JOINT_TEMPLATE.format(BOTTOM_DRAWER) init = [ ('BConf', init_bq), ('AtBConf', init_bq), ('AConf', init_bq, carry_aq), #('RestAConf', carry_aq), #('AConf', init_bq, calibrate_aq), ( 'Stationary', ), ('AConf', init_bq, init_aq), ('AtAConf', init_aq), ('GConf', open_gq), ('GConf', closed_gq), ('Grasp', None, None), ('AtGrasp', None, None), ('Above', top_joint, bottom_joint), ('Adjacent', top_joint, bottom_joint), ('Adjacent', bottom_joint, top_joint), ('Calibrated', ), ('CanMoveBase', ), ('CanMoveArm', ), ('CanMoveGripper', ), ] + list(task.init) + list(additional_init) for action_name, cost in ACTION_COSTS.items(): function_name = '{}Cost'.format(title_from_snake(action_name)) function = (function_name, ) init.append(Equal(function, cost)) # TODO: stove state init += [('Stackable', name, surface) for name, surface in task.goal_on.items()] + \ [('Stackable', name, stove) for name, stove in product(task.goal_cooked, STOVES)] + \ [('Pressed', name) for name in belief.pressed] + \ [('Cookable', name) for name in task.goal_cooked] + \ [('Cooked', name) for name in belief.cooked] + \ [('Status', status) for status in DOOR_STATUSES] + \ [('Knob', knob) for knob in KNOBS] + \ [('Joint', knob) for knob in KNOBS] + \ [('Liquid', liquid) for _, liquid in task.init_liquid] + \ [('HasLiquid', cup, liquid) for cup, liquid in belief.liquid] + \ [('StoveKnob', STOVE_TEMPLATE.format(loc), KNOB_TEMPLATE.format(loc)) for loc in STOVE_LOCATIONS] + \ [('GraspType', ty) for ty in task.grasp_types] # TODO: grasp_type per object #[('Type', obj_name, 'stove') for obj_name in STOVES] + \ #[('Camera', name) for name in world.cameras] if task.movable_base: init.append(('MovableBase', )) if fixed_base: init.append(('InitBConf', init_bq)) if task.noisy_base: init.append(('NoisyBase', )) compute_pose_kin = get_compute_pose_kin(world) compute_angle_kin = get_compute_angle_kin(world) initial_poses = {} for joint_name, init_conf in belief.door_confs.items(): if joint_name in DRAWER_JOINTS: init.append(('Drawer', joint_name)) if joint_name in CABINET_JOINTS: init.append(('Cabinet', joint_name)) joint = joint_from_name(world.kitchen, joint_name) surface_name = surface_from_joint(joint_name) init.append(('SurfaceJoint', surface_name, joint_name)) # Relies on the fact that drawers have identical surface and link names link_name = get_link_name(world.kitchen, child_link_from_joint(joint)) #link_name = str(link_name.decode('UTF-8')) #link_name = str(link_name.encode('ascii','ignore')) for conf in { init_conf, world.open_kitchen_confs[joint], world.closed_kitchen_confs[joint] }: # TODO: return to initial poses? world_pose, = compute_angle_kin(link_name, joint_name, conf) init.extend([ ('Joint', joint_name), ('Angle', joint_name, conf), ('Obstacle', link_name), ('AngleKin', link_name, world_pose, joint_name, conf), ('WorldPose', link_name, world_pose), ]) if joint in world.kitchen_joints: init.extend([ ('Sample', world_pose), #('Value', world_pose), # comment out? ]) if conf == init_conf: initial_poses[link_name] = world_pose init.extend([ ('AtAngle', joint_name, conf), ('AtWorldPose', link_name, world_pose), ]) for surface_name in ALL_SURFACES: if surface_name in OPEN_SURFACES: init.append(('Counter', surface_name)) # Fixed surface if surface_name in DRAWERS: init.append(('Drawer', surface_name)) surface = surface_from_name(surface_name) surface_link = link_from_name(world.kitchen, surface.link) parent_joint = parent_joint_from_link(surface_link) if parent_joint not in world.kitchen_joints: # TODO: attach to world frame? world_pose = RelPose(world.kitchen, surface_link, init=True) initial_poses[surface_name] = world_pose init += [ #('RelPose', surface_name, world_pose, 'world'), ('WorldPose', surface_name, world_pose), #('AtRelPose', surface_name, world_pose, 'world'), ('AtWorldPose', surface_name, world_pose), ('Sample', world_pose), #('Value', world_pose), ] init.extend([ ('CheckNearby', surface_name), #('InitPose', world_pose), ('Localized', surface_name), ]) for grasp_type in task.grasp_types: if (surface_name in OPEN_SURFACES) or has_place_database( world.robot_name, surface_name, grasp_type): init.append(('AdmitsGraspType', surface_name, grasp_type)) if belief.grasped is None: init.extend([ ('HandEmpty', ), ('GConf', init_gq), ('AtGConf', init_gq), ]) else: obj_name = belief.grasped.body_name assert obj_name not in belief.pose_dists grasp = belief.grasped init += [ # Static #('Graspable', obj_name), ('Grasp', obj_name, grasp), ('IsGraspType', obj_name, grasp, grasp.grasp_type), # Fluent ('AtGrasp', obj_name, grasp), ('Holding', obj_name), ('Localized', obj_name), ] init.extend(('ValidGraspType', obj_name, grasp_type) for grasp_type in task.grasp_types if implies(world.is_real(), is_valid_grasp_type(obj_name, grasp_type))) for obj_name in world.movable: obj_type = type_from_name(obj_name) if obj_type in BOWLS: init.append(('Bowl', obj_name)) else: init.append( ('Obstacle', obj_name)) # TODO: hack to place within bowls if obj_type in COOKABLE: init.append(('Cookable', obj_name)) if obj_type in POURABLE: init.append(('Pourable', obj_name)) init += [ ('Entity', obj_name), ('CheckNearby', obj_name), ] + [('Stackable', obj_name, counter) for counter in set(ALL_SURFACES) & set(COUNTERS)] # TODO: track poses over time to produce estimates for obj_name, pose_dist in belief.pose_dists.items(): dist_support = pose_dist.dist.support() localized = pose_dist.is_localized() graspable = True if localized: init.append(('Localized', obj_name)) [rel_pose] = dist_support roll, pitch, yaw = euler_from_quat( quat_from_pose(rel_pose.get_reference_from_body())) if (MAX_ERROR < abs(roll)) or (MAX_ERROR < abs(pitch)): graspable = False print( '{} has an invalid orientation: roll={:.3f}, pitch={:.3f}'. format(obj_name, roll, pitch)) if graspable: #init.append(('Graspable', obj_name)) init.extend(('ValidGraspType', obj_name, grasp_type) for grasp_type in task.grasp_types if implies(world.is_real(), is_valid_grasp_type(obj_name, grasp_type))) # Could also fully decompose into points (but many samples) # Could immediately add likely points for collision checking for rel_pose in (dist_support if localized else pose_dist.decompose()): surface_name = rel_pose.support if surface_name is None: # Treats as obstacle # TODO: could temporarily add to fixed world_pose = rel_pose init += [ ('WorldPose', obj_name, world_pose), ('AtWorldPose', obj_name, world_pose), ] poses = [world_pose] #raise RuntimeError(obj_name, supporting) else: surface_pose = initial_poses[surface_name] world_pose, = compute_pose_kin(obj_name, rel_pose, surface_name, surface_pose) init += [ # Static ('RelPose', obj_name, rel_pose, surface_name), ('WorldPose', obj_name, world_pose), ('PoseKin', obj_name, world_pose, rel_pose, surface_name, surface_pose), # Fluent ('AtRelPose', obj_name, rel_pose, surface_name), ('AtWorldPose', obj_name, world_pose), ] if localized: init.append(('On', obj_name, surface_name)) poses = [rel_pose, world_pose] for pose in poses: if isinstance(pose, PoseDist): init.append(('Dist', pose)) else: init.extend([('Sample', pose)]) #, ('Value', pose)]) #for body, ty in problem.body_types: # init += [('Type', body, ty)] #bodies_from_type = get_bodies_from_type(problem) #bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter(ty) else [ty] goal_formula = get_goal(belief, init) stream_pddl, stream_map = get_streams(world, teleport_base=task.teleport_base, **kwargs) print('Constants:', constant_map) print('Init:', sorted(init, key=lambda f: f[0])) print('Goal:', goal_formula) #print('Streams:', stream_map.keys()) # DEBUG return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
def solve_focused(problem, constraints=PlanConstraints(), stream_info={}, action_info={}, max_time=INF, max_iterations=INF, 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 action_info: a dictionary from stream name to ActionInfo for planning and execution :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 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 = float(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) full_action_info = get_action_info(action_info) 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, 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) combined_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: 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) 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()