def visualize_stream_orders(orders, streams=[], filename='stream_orders' + DEFAULT_EXTENSION): from pygraphviz import AGraph graph = AGraph(strict=True, directed=True) graph.node_attr['style'] = 'filled' graph.node_attr['shape'] = 'box' graph.node_attr['color'] = STREAM_COLOR graph.node_attr['fontcolor'] = 'black' #graph.node_attr['fontsize'] = 12 graph.node_attr['width'] = 0 graph.node_attr['height'] = 0.02 # Minimum height is 0.02 graph.node_attr['margin'] = 0 graph.graph_attr['outputMode'] = 'nodesfirst' graph.graph_attr['dpi'] = 300 streams = set(streams) | set(flatten(orders)) for stream in streams: graph.add_node(str(stream)) for stream1, stream2 in orders: graph.add_edge(str(stream1), str(stream2)) # TODO: could also print the raw values (or a lookup table) # https://stackoverflow.com/questions/3499056/making-a-legend-key-in-graphviz graph.draw(filename, prog='dot') print('Saved', filename) #display_image(filename) return graph
def instantiate_condition(action, is_static, args_from_predicate): parameters = {p.name for p in action.parameters} #if not parameters: # yield {} # return static_conditions = list( filter(is_static, get_literals(get_precondition(action)))) static_parameters = set( filter(is_parameter, flatten(atom.args for atom in static_conditions))) if not (parameters <= static_parameters): raise NotImplementedError( 'Could not instantiate action {} due to parameters: {}'.format( action.name, str_from_object(parameters - static_parameters))) atoms_from_cond = { condition: args_from_predicate[condition.predicate, get_constants(condition)] for condition in static_conditions } conditions, atoms = zip(*atoms_from_cond.items()) relations = [ Relation(conditions[index].args, atoms[index]) for index in compute_order(conditions, atoms) ] solution = solve_satisfaction(relations) for element in solution.body: yield solution.get_mapping(element)
def create_disable_axiom(external_plan, use_parameters=True): # TODO: express constraint mutexes upfront # TODO: investigate why use_parameters=True hurts satisfaction # TODO: better mix optimization and sampling by determining a splitting point # TODO: be careful about the shared objects as parameters # TODO: need to block functions & predicates stream_plan, _ = partition_external_plan(external_plan) assert stream_plan #component_plan = stream_plan [unsatisfiable] = stream_plan[-1].get_unsatisfiable() component_plan = list(flatten( r.get_components() for r in stream_plan[:-1])) + list(unsatisfiable) increase_free_variables(component_plan) #output_objects = get_free_objects(component_plan) if use_parameters else set() constraints = [result.stream_fact for result in component_plan] optimistic_objects = { o for f in constraints for o in get_args(f) if isinstance(o, OptimisticObject) } # TODO: consider case when variables are free #assert optimistic_objects <= output_objects #free_objects = list(optimistic_objects & output_objects) # TODO: need to return all variables free_objects = optimistic_objects parameters = ['?p{}'.format(i) for i in range(len(free_objects))] param_from_obj = get_mapping(free_objects, parameters) preconditions = substitute_expression(constraints, param_from_obj) effect = (UNSATISFIABLE, ) axiom = make_axiom(parameters, preconditions, effect) #axiom.dump() return axiom
def get_problem1(n_regions=0, n_cylinders=3, n_boxes=3): constant_map = {} stream_map = DEBUG initial = 'initial' shelfA = 'shelfA' prepushA = 'prepushA' shelfB = 'shelfB' regions = [initial, shelfA, prepushA, shelfB] + \ ['region{}'.format(i) for i in range(n_regions)] cylinders = ['cylinder{}'.format(i) for i in range(n_cylinders)] boxes = ['box{}'.format(i) for i in range(n_boxes)] init = [ ('HandEmpty',), ('Prepush', prepushA, shelfA), #('Stackable', cylinders[2], cylinders[1]), ] init += [('Region', region) for region in regions] init += [('Cylinder', cylinder) for cylinder in cylinders] init += [('Box', box) for box in boxes] init.extend(flatten([('Movable', movable), ('On', movable, initial), ('Clear', movable)] for movable in (cylinders + boxes))) init += [('Thing', thing) for thing in (regions + cylinders + boxes)] goal = And( ('In', cylinders[0], shelfA), ('On', cylinders[2], cylinders[1]), ) return PDDLProblem(DOMAIN_PDDL, constant_map, STREAM_PDDL, stream_map, init, goal)
def shorten_stream_plan(evaluations, stream_plan, target_facts): all_subgoals = set(target_facts) | set( flatten(r.instance.get_domain() for r in stream_plan)) evaluation_subgoals = set( filter(evaluations.__contains__, map(evaluation_from_fact, all_subgoals))) open_subgoals = set( filter(lambda f: evaluation_from_fact(f) not in evaluations, all_subgoals)) results_from_fact = {} for result in stream_plan: for fact in result.get_certified(): results_from_fact.setdefault(fact, []).append(result) for removed_result in reversed(stream_plan): # TODO: only do in order? certified_subgoals = open_subgoals & set( removed_result.get_certified()) if not certified_subgoals: # Could combine with following new_stream_plan = stream_plan[:] new_stream_plan.remove(removed_result) return new_stream_plan if all(2 <= len(results_from_fact[fact]) for fact in certified_subgoals): node_from_atom = get_achieving_streams( evaluation_subgoals, set(stream_plan) - {removed_result}) if all(fact in node_from_atom for fact in target_facts): new_stream_plan = [] extract_stream_plan(node_from_atom, target_facts, new_stream_plan) return new_stream_plan return None
def recover_axioms_plans(instantiated, action_instances): #axioms, axiom_init, _ = axiom_rules.handle_axioms( # instantiated.actions, instantiated.axioms, instantiated.goal_list) new_action_instances = [ copy.deepcopy(instance) for instance in action_instances ] axioms, axiom_init = instantiated.axioms, [ ] # TODO: bug when needing to reachieve negated axioms_from_effect = defaultdict(list) for axiom in axioms: axioms_from_effect[axiom.effect].append(axiom) axioms_from_name = get_derived_predicates(instantiated.task.axioms) state = set(instantiated.task.init) | set(axiom_init) axiom_plans = [] for action in new_action_instances + [ get_goal_instance(instantiated.task.goal) ]: all_conditions = list(get_precondition(action)) + list( flatten(cond for cond, _ in action.add_effects + action.del_effects)) axioms = backtrack_axioms(all_conditions, axioms_from_effect, set()) axiom_from_atom, _ = get_achieving_axioms(state, axioms) action.applied_effects = [] for effects in [action.add_effects, action.del_effects]: negate = (effects is action.del_effects) for i, (conditions, effect) in reversed(list(enumerate(effects))): if all( literal_holds(state, literal) or ( literal in axiom_from_atom) for literal in conditions): action.precondition.extend(conditions) effects[i] = ([], effect) action.applied_effects.append( effect.negate() if negate else effect) else: effects.pop(i) # RuntimeError: Preimage fact ('new-axiom@0',) is not achievable! #precondition = action.precondition # TODO: strange bug if this applies precondition = [ literal for literal in action.precondition if literal.predicate in axioms_from_name ] axiom_plans.append([]) success = extract_axioms(state, axiom_from_atom, precondition, axiom_plans[-1]) if not success: print(all_conditions) print(action) print(axioms) raise RuntimeError('Could not extract axioms') apply_action(state, action) return new_action_instances, axiom_plans
def save_segmented(image, beads_per_color): if image is None: return all_beads = set(flatten(beads_per_color)) non_beads = sorted(set(get_bodies()) - all_beads) color_from_body = dict(zip(non_beads, spaced_colors(len(non_beads)))) segmented_image = image_from_segmented(image, color_from_body=color_from_body) save_image('segmented.png', segmented_image)
def increase_free_variables(stream_plan): # TODO: could decrease the number of variables if a cluster is removed free_objects = Counter( flatten(result.instance.input_objects for result in stream_plan)) for obj, num in free_objects.items(): # TODO: wait until the full plan has failed (accomplished through levels) if isinstance(obj, OptimisticObject): assert isinstance(obj.param, UniqueOptValue) instance = obj.param.instance instance.num_optimistic = max(instance.num_optimistic, num + 1)
def score_image(rgb_image, bead_colors, beads_per_color, max_distance=0.1): # TODO: could floodfill to identify bead clusters (and reward more clusters) # TODO: ensure the appropriate ratios are visible on top # TODO: penalize marbles that have left the bowl # TODO: estimate the number of escaped marbles using the size at that distance bead_pixels = [[] for _ in bead_colors] for r in range(rgb_image.shape[0]): # height for c in range(rgb_image.shape[1]): # width pixel = rgb_image[r, c] assert pixel[3] == 255 rgb = pixel[:3] / 255. best_index, best_distance = None, INF for index, color in enumerate(bead_colors): distance = np.linalg.norm(rgb - color[:3]) if distance < best_distance: best_index, best_distance = index, distance if best_distance <= max_distance: bead_pixels[best_index].append((r, c)) # TODO: discount beads outside all_beads = list(flatten(beads_per_color)) bead_frequencies = np.array([len(beads) for beads in beads_per_color], dtype=float) / len(all_beads) all_pixels = list(flatten(bead_pixels)) image_frequencies = np.array([len(beads) for beads in bead_pixels], dtype=float) / len(all_pixels) print(bead_frequencies, image_frequencies, image_frequencies - bead_frequencies) distances = [] for pixels in bead_pixels: distances.extend([ get_distance(p1, p2, norm=2) for p1, p2 in combinations(pixels, r=2) ]) dispersion = np.mean(distances) # TODO: translate into meters? print(dispersion)
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
def recover_axioms_plans(instantiated, action_instances): #axioms, axiom_init, _ = axiom_rules.handle_axioms( # instantiated.actions, instantiated.axioms, instantiated.goal_list) axioms, axiom_init = instantiated.axioms, [ ] # TODO: bug when needing to reachieve negated axioms_from_effect = defaultdict(list) for axiom in axioms: axioms_from_effect[axiom.effect].append(axiom) state = set(instantiated.task.init) | set(axiom_init) axiom_plans = [] for action in action_instances + [ get_goal_instance(instantiated.task.goal) ]: all_conditions = list(get_precondition(action)) + list( flatten(cond for cond, _ in action.add_effects + action.del_effects)) axioms = backtrack_axioms(all_conditions, axioms_from_effect, set()) axiom_from_atom, _ = get_achieving_axioms(state, axioms) action.applied_effects = [] for effects in [action.add_effects, action.del_effects]: negate = effects is action.del_effects for i, (conditions, effect) in reversed(list(enumerate(effects))): if all( literal_holds(state, literal) or ( literal in axiom_from_atom) for literal in conditions): action.precondition.extend(conditions) effects[i] = ([], effect) action.applied_effects.append( effect.negate() if negate else effect) else: effects.pop(i) axiom_plans.append([]) assert extract_axioms(state, axiom_from_atom, action.precondition, axiom_plans[-1]) apply_action(state, action) return axiom_plans
def get_free_objects(stream_plan): return set( flatten(result.output_objects for result in stream_plan if isinstance(result, StreamResult)))
def recover_stream_plan(evaluations, current_plan, opt_evaluations, goal_expression, domain, node_from_atom, action_plan, axiom_plans, negative, replan_step): # Universally quantified conditions are converted into negative axioms # Existentially quantified conditions are made additional preconditions # Universally quantified effects are instantiated by doing the cartesian produce of types (slow) # Added effects cancel out removed effects # TODO: node_from_atom is a subset of opt_evaluations (only missing functions) real_task = task_from_domain_problem( domain, get_problem(evaluations, goal_expression, domain)) opt_task = task_from_domain_problem( domain, get_problem(opt_evaluations, goal_expression, domain)) negative_from_name = { external.blocked_predicate: external for external in negative if external.is_negated() } real_states, combined_plan = recover_negative_axioms( real_task, opt_task, axiom_plans, action_plan, negative_from_name) function_plan = compute_function_plan(opt_evaluations, action_plan) # TODO: record the supporting facts full_preimage = plan_preimage(combined_plan, []) stream_preimage = set(full_preimage) - real_states[0] negative_preimage = set( filter(lambda a: a.predicate in negative_from_name, stream_preimage)) function_plan.update( convert_negative(negative_preimage, negative_from_name, full_preimage, real_states)) positive_preimage = stream_preimage - negative_preimage steps_from_fact = { fact_from_fd(l): full_preimage[l] for l in positive_preimage if not l.negated } target_facts = { fact for fact in steps_from_fact.keys() if get_prefix(fact) != EQ } #stream_plan = reschedule_stream_plan(evaluations, target_facts, domain, stream_results) # visualize_constraints(map(fact_from_fd, target_facts)) # TODO: get_steps_from_stream stream_plan = [] step_from_stream = {} for result in current_plan: # TODO: actually compute when these are needed + dependencies step_from_stream[result] = 0 if isinstance(result.external, Function) or (result.external in negative): function_plan.add( result) # Prevents these results from being pruned else: stream_plan.append(result) curr_evaluations = evaluations_from_stream_plan(evaluations, stream_plan, max_effort=None) extraction_facts = target_facts - set( map(fact_from_evaluation, curr_evaluations)) step_from_fact = { fact: min(steps_from_fact[fact]) for fact in extraction_facts } extract_stream_plan(node_from_atom, extraction_facts, stream_plan, step_from_fact, step_from_stream) stream_plan = postprocess_stream_plan(evaluations, domain, stream_plan, target_facts) eager_plan = [] actions_from_step = {} for result in (stream_plan + list(function_plan)): if (result.opt_index != 0) or (step_from_stream.get(result, 0) < replan_step): eager_plan.append(result) else: actions_from_step.setdefault(step_from_stream[result], []).append(result.get_action()) eager_plan = convert_fluent_streams(eager_plan, real_states, action_plan, steps_from_fact, node_from_atom) # print(action_plan) # # TODO: propagate this forward in the future # start_from_stream = {} # for result in eager_plan: # stuff = list(map(fd_from_fact, get_fluent_domain(result))) # index = len(real_states) # for i, state in enumerate(real_states): # if conditions_hold(state, stuff): # start_from_stream[result] = i # index = i # break # #else: # #start_from_stream[result] = len(real_states) # print(index, result) # TODO: some sort of obj side-effect bug that requires obj_from_pddl to be applied last (likely due to fluent streams) #action_plan = transform_plan_args(map(pddl_from_instance, action_instances), obj_from_pddl) for step, action in enumerate(action_plan): actions_from_step.setdefault(step, []).append( transform_action_args(pddl_from_instance(action), obj_from_pddl)) action_plan = list( flatten(actions_from_step[step] for step in sorted(actions_from_step))) return eager_plan, action_plan