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
