def fn(outputs, certified): assert (len(outputs) == 1) q0, _, q1, o, g = find_unique(lambda f: f[0] == 'holdingmotion', certified)[1:] obstacles = fixed + place_movable(certified) holding_motion_fn = get_holding_motion_gen(robot, obstacles, teleport) return holding_motion_fn(q0, q1, o, g)
def get_action_instances(task, action_plan): import pddl import instantiate type_to_objects = instantiate.get_objects_by_type(task.objects, task.types) function_assignments = { f.fluent: f.expression for f in task.init if isinstance(f, pddl.f_expression.FunctionAssignment) } fluent_facts = MockSet() init_facts = set() action_instances = [] for name, objects in action_plan: # TODO: what if more than one action of the same name due to normalization? # Normalized actions have same effects, so I just have to pick one action = find_unique(lambda a: a.name == name, task.actions) args = map(pddl_from_object, objects) assert (len(action.parameters) == len(args)) variable_mapping = {p.name: a for p, a in zip(action.parameters, args)} instance = action.instantiate(variable_mapping, init_facts, fluent_facts, type_to_objects, task.use_min_cost_metric, function_assignments) assert (instance is not None) action_instances.append(instance) return action_instances
def process_conditional_effect(effect, negative_from_predicate): import pddl new_parts = [] stream_facts = [] for disjunctive in get_conjunctive_parts(effect.condition): for literal in get_disjunctive_parts(disjunctive): # TODO: assert only one disjunctive part if isinstance(literal, pddl.Literal) and (literal.predicate in negative_from_predicate): stream = negative_from_predicate[literal.predicate] if not isinstance(stream, ConstraintStream): new_parts.append(literal) continue certified = find_unique( lambda f: get_prefix(f) == literal.predicate, stream.certified) mapping = get_mapping(get_args(certified), literal.args) stream_facts.append( fd_from_fact( substitute_expression(stream.stream_fact, mapping))) # TODO: add the negated literal as precondition here? else: new_parts.append(literal) return new_parts, stream_facts
def remap_certified(literal, stream): certified = find_unique(lambda f: get_prefix(f) == literal.predicate, stream.certified) mapping = get_mapping(get_args(certified), literal.args) if not all(arg in mapping for arg in stream.inputs): # Certified must contain all inputs return None return mapping
def get_action_cost(domain, results_from_head, name, args): import pddl action = find_unique(lambda a: a.name == name, domain.actions) if action.cost is None: return 0. if isinstance(action.cost.expression, pddl.NumericConstant): return action.cost.expression.value / get_cost_scale() var_mapping = {p.name: a for p, a in zip(action.parameters, args)} args = tuple(var_mapping[p] for p in action.cost.expression.args) head = Head(action.cost.expression.symbol, args) [(value, _)] = results_from_head[head] return value
def extract_function_plan(function_evaluations, action_plan, domain, unit_costs): if unit_costs: return [] function_plan = set() results_from_head = get_results_from_head(function_evaluations) for name, args in action_plan: action = find_unique(lambda a: a.name == name, domain.actions) pddl_args = tuple(map(pddl_from_object, args)) result = extract_function_results(results_from_head, action, pddl_args) if result is not None: function_plan.add(result) return list(function_plan)
def optimizer_conditional_effects(domain, externals): import pddl #from pddlstream.algorithms.scheduling.negative import get_negative_predicates # TODO: extend this to predicates if UNIVERSAL_TO_CONDITIONAL: negative_streams = list(filter(lambda e: e.is_negated(), externals)) else: negative_streams = list( filter( lambda e: isinstance(e, ConstraintStream) and e.is_negated(), externals)) negative_from_predicate = get_predicate_map(negative_streams) if not negative_from_predicate: return for action in domain.actions: universal_to_conditional(action) new_effects = [] for effect in action.effects: if effect.literal.predicate != UNSATISFIABLE: new_effects.append(effect) continue new_parts = [] stream_facts = [] for disjunctive in get_conjunctive_parts(effect.condition): for literal in get_disjunctive_parts(disjunctive): # TODO: assert only one disjunctive part if isinstance(literal, pddl.Literal) and ( literal.predicate in negative_from_predicate): stream = negative_from_predicate[literal.predicate] if not isinstance(stream, ConstraintStream): new_parts.append(literal) continue certified = find_unique( lambda f: get_prefix(f) == literal.predicate, stream.certified) mapping = get_mapping(get_args(certified), literal.args) stream_facts.append( fd_from_fact( substitute_expression(stream.stream_fact, mapping))) # TODO: add the negated literal as precondition here? else: new_parts.append(literal) if not stream_facts: new_effects.append(effect) for stream_fact in stream_facts: new_effects.append( pddl.Effect(effect.parameters, pddl.Conjunction(new_parts), stream_fact)) action.effects = new_effects
def fn(literal): if literal.predicate not in predicate_map: return literal # TODO: other checks on only inputs stream = predicate_map[literal.predicate] certified = find_unique(lambda f: get_prefix(f) == literal.predicate, stream.certified) mapping = get_mapping(get_args(certified), literal.args) #assert all(arg in mapping for arg in stream.inputs) # Certified must contain all inputs if not all(arg in mapping for arg in stream.inputs): # TODO: this excludes typing. This is not entirely safe return literal blocked_args = tuple(mapping[arg] for arg in stream.inputs) blocked_literal = literal.__class__(stream.blocked_predicate, blocked_args).negate() if stream.is_negated(): # TODO: add stream conditions here return blocked_literal return pddl.Conjunction([literal, blocked_literal])
def get_action_instances(task, action_plan): type_to_objects = instantiate.get_objects_by_type(task.objects, task.types) function_assignments = get_function_assignments(task) predicate_to_atoms = instantiate.get_atoms_by_predicate(task.init) fluent_facts = MockSet() init_facts = set() action_instances = [] for name, objects in action_plan: # TODO: what if more than one action of the same name due to normalization? # Normalized actions have same effects, so I just have to pick one # TODO: conditional effects and internal parameters action = find_unique(lambda a: a.name == name, task.actions) args = list(map(pddl_from_object, objects)) variable_mapping = {p.name: a for p, a in safe_zip(action.parameters, args)} instance = action.instantiate(variable_mapping, init_facts, fluent_facts, type_to_objects, task.use_min_cost_metric, function_assignments, predicate_to_atoms) assert (instance is not None) action_instances.append(instance) return action_instances
def simplify_actions(opt_evaluations, action_plan, task, actions, unit_costs): # TODO: add ordering constraints to simplify the optimization import pddl import instantiate fluent_facts = MockSet() init_facts = set() type_to_objects = instantiate.get_objects_by_type(task.objects, task.types) results_from_head = get_results_from_head(opt_evaluations) action_from_name = {} function_plan = set() for i, (name, args) in enumerate(action_plan): action = find_unique(lambda a: a.name == name, actions) assert (len(action.parameters) == len(args)) # parameters = action.parameters[:action.num_external_parameters] var_mapping = {p.name: a for p, a in zip(action.parameters, args)} new_name = '{}-{}'.format(name, i) new_parameters = action.parameters[len(args):] new_preconditions = [] action.precondition.instantiate(var_mapping, init_facts, fluent_facts, new_preconditions) new_effects = [] for eff in action.effects: eff.instantiate(var_mapping, init_facts, fluent_facts, type_to_objects, new_effects) new_effects = [pddl.Effect([], pddl.Conjunction(conditions), effect) for conditions, effect in new_effects] cost = pddl.Increase(fluent=pddl.PrimitiveNumericExpression(symbol=TOTAL_COST, args=[]), expression=pddl.NumericConstant(1)) # cost = None task.actions.append(pddl.Action(new_name, new_parameters, len(new_parameters), pddl.Conjunction(new_preconditions), new_effects, cost)) action_from_name[new_name] = (name, map(obj_from_pddl, args)) if not unit_costs: function_result = extract_function_results(results_from_head, action, args) if function_result is not None: function_plan.add(function_result) return action_from_name, list(function_plan)
def optimizer_conditional_effects(domain, externals): import pddl #from pddlstream.algorithms.scheduling.negative import get_negative_predicates # TODO: extend this to predicates negative_streams = list( filter(lambda e: isinstance(e, ConstraintStream) and e.is_negated(), externals)) negative_from_predicate = get_predicate_map(negative_streams) if not negative_from_predicate: return for action in domain.actions: universal_to_conditional(action) for effect in action.effects: if isinstance(effect, pddl.Effect) and (effect.literal.predicate == UNSATISFIABLE): condition = effect.condition new_parts = [] stream_fact = None for literal in get_conjuctive_parts(condition): if isinstance(literal, pddl.Literal) and ( literal.predicate in negative_from_predicate): if stream_fact is not None: raise NotImplementedError() stream = negative_from_predicate[literal.predicate] certified = find_unique( lambda f: get_prefix(f) == literal.predicate, stream.certified) mapping = get_mapping(get_args(certified), literal.args) stream_fact = substitute_expression( stream.stream_fact, mapping) else: new_parts.append(literal) if stream_fact is not None: effect.condition = pddl.Conjunction(new_parts) effect.literal = fd_from_fact(stream_fact)
def fn(outputs, certified): assert (len(outputs) == 1) q0, _, q1 = find_unique(lambda f: f[0] == 'basemotion', certified)[1:] place_movable(certified) free_motion_fn = get_motion_gen(problem, teleport) return free_motion_fn(q0, q1)
def add_plan_constraints(constraints, domain, evaluations, goal_exp, internal=False): if (constraints is None) or (constraints.skeletons is None): return goal_exp import pddl # TODO: can search over skeletons first and then fall back # TODO: unify this with the constraint ordering # TODO: can constrain to use a plan prefix prefix = '_' if internal else '' assigned_predicate = ASSIGNED_PREDICATE.format(prefix) group_predicate = GROUP_PREDICATE.format(prefix) order_predicate = ORDER_PREDICATE.format(prefix) for group in constraints.groups: for value in constraints.groups[group]: # TODO: could make all constants groups (like an equality group) fact = (group_predicate, to_obj(group), to_obj(value)) add_fact(evaluations, fact, result=INTERNAL_EVALUATION) new_actions = [] new_goals = [] for num, skeleton in enumerate(constraints.skeletons): # TODO: change the prefix for these order_facts = [(order_predicate, to_obj('n{}'.format(num)), to_obj('t{}'.format(step))) for step in range(len(skeleton) + 1)] add_fact(evaluations, order_facts[0], result=INTERNAL_EVALUATION) new_goals.append(order_facts[-1]) bound_parameters = set() for step, (name, args) in enumerate(skeleton): # TODO: could also just remove the free parameter from the action new_action = deepcopy( find_unique(lambda a: a.name == name, domain.actions)) constant_pairs = [(a, p.name) for a, p in safe_zip(args, new_action.parameters) if not is_parameter(a) and a != WILD] skeleton_parameters = list(filter(is_parameter, args)) existing_parameters = [ p for p in skeleton_parameters if p in bound_parameters ] local_from_global = { a: p.name for a, p in safe_zip(args, new_action.parameters) if is_parameter(a) } group_preconditions = [ (group_predicate if is_hashable(a) and (a in constraints.groups) else EQ, to_obj(a), p) for a, p in constant_pairs ] new_preconditions = make_assignment_facts(assigned_predicate, local_from_global, existing_parameters) + \ group_preconditions + [order_facts[step]] new_action.precondition = pddl.Conjunction([ new_action.precondition, make_preconditions(new_preconditions) ]).simplified() new_effects = make_assignment_facts(assigned_predicate, local_from_global, skeleton_parameters) \ + [Not(order_facts[step]), order_facts[step + 1]] new_action.effects.extend(make_effects(new_effects)) # TODO: should also negate the effects of all other sequences here new_actions.append(new_action) bound_parameters.update(skeleton_parameters) #new_action.dump() add_predicate(domain, make_predicate(order_predicate, ['?num', '?step'])) if constraints.exact: domain.actions[:] = [] domain.actions.extend(new_actions) new_goal_exp = And(goal_exp, Or(*new_goals)) return new_goal_exp
def sequential_stream_plan(evaluations, goal_expression, domain, stream_results, negated, unit_costs=True, **kwargs): if negated: raise NotImplementedError() # TODO: compute preimage and make that the goal instead opt_evaluations = evaluations_from_stream_plan(evaluations, stream_results) opt_task = task_from_domain_problem(domain, get_problem(opt_evaluations, goal_expression, domain, unit_costs)) action_plan, action_cost = solve_from_task(opt_task, **kwargs) if action_plan is None: return None, action_cost import instantiate fluent_facts = MockSet() init_facts = set() task = task_from_domain_problem(domain, get_problem(evaluations, goal_expression, domain, unit_costs)) type_to_objects = instantiate.get_objects_by_type(task.objects, task.types) task.actions, stream_result_from_name = get_stream_actions(stream_results) results_from_head = get_results_from_head(opt_evaluations) # TODO: add ordering constraints to simplify the optimization import pddl action_from_name = {} function_plan = set() for i, (name, args) in enumerate(action_plan): action = find_unique(lambda a: a.name == name, domain.actions) assert(len(action.parameters) == len(args)) #parameters = action.parameters[:action.num_external_parameters] var_mapping = {p.name: a for p, a in zip(action.parameters, args)} new_name = '{}-{}'.format(name, i) new_parameters = action.parameters[len(args):] new_preconditions = [] action.precondition.instantiate(var_mapping, init_facts, fluent_facts, new_preconditions) new_effects = [] for eff in action.effects: eff.instantiate(var_mapping, init_facts, fluent_facts, type_to_objects, new_effects) new_effects = [pddl.Effect([], pddl.Conjunction(conditions), effect) for conditions, effect in new_effects] cost = pddl.Increase(fluent=pddl.PrimitiveNumericExpression(symbol=TOTAL_COST, args=[]), expression=pddl.NumericConstant(1)) #cost = None task.actions.append(pddl.Action(new_name, new_parameters, 0, pddl.Conjunction(new_preconditions), new_effects, cost)) action_from_name[new_name] = (name, map(obj_from_pddl, args)) if not unit_costs: function_plan.update(extract_function_results(results_from_head, action, args)) planner = kwargs.get('planner', 'ff-astar') combined_plan, _ = solve_from_task(task, planner=planner, **kwargs) if combined_plan is None: return None, obj_from_pddl_plan(action_plan), INF stream_plan = [] action_plan = [] for name, args in combined_plan: if name in stream_result_from_name: stream_plan.append(stream_result_from_name[name]) else: action_plan.append(action_from_name[name]) stream_plan += list(function_plan) combined_plan = stream_plan + action_plan return combined_plan, action_cost
def add_plan_constraints(constraints, domain, evaluations, goal_exp, internal=False): if (constraints is None) or (constraints.skeletons is None): return goal_exp import pddl # TODO: unify this with the constraint ordering # TODO: can constrain to use a plan prefix prefix = get_internal_prefix(internal) assigned_predicate = ASSIGNED_PREDICATE.format(prefix) bound_predicate = BOUND_PREDICATE.format(prefix) group_predicate = GROUP_PREDICATE.format(prefix) order_predicate = ORDER_PREDICATE.format(prefix) new_facts = [] for group in constraints.groups: for value in constraints.groups[group]: # TODO: could make all constants groups (like an equality group) fact = (group_predicate, to_obj(group), to_obj(value)) new_facts.append(fact) new_actions = [] new_goals = [] for num, skeleton in enumerate(constraints.skeletons): actions, orders = skeleton incoming_orders, _ = neighbors_from_orders(orders) order_facts = [(order_predicate, to_obj('n{}'.format(num)), to_obj('t{}'.format(step))) for step in range(len(actions))] for step, (name, args) in enumerate(actions): # TODO: could also just remove the free parameter from the action new_action = deepcopy( find_unique(lambda a: a.name == name, domain.actions)) local_from_global = { a: p.name for a, p in safe_zip(args, new_action.parameters) if is_parameter(a) } ancestors, descendants = get_ancestors(step, orders), get_descendants( step, orders) parallel = set(range( len(actions))) - ancestors - descendants - {step} parameters = set(filter(is_parameter, args)) ancestor_parameters = parameters & set( filter(is_parameter, (p for idx in ancestors for p in actions[idx][1]))) #descendant_parameters = parameters & set(filter(is_parameter, (p for idx in descendants for p in actions[idx][1]))) parallel_parameters = parameters & set( filter(is_parameter, (p for idx in parallel for p in actions[idx][1]))) #bound_preconditions = [Imply(bound, assigned) for bound, assigned in safe_zip(bound_facts, assigned_facts)] bound_condition = pddl.Conjunction([ pddl.Disjunction( map(fd_from_fact, [ Not((bound_predicate, to_constant(p))), (assigned_predicate, to_constant(p), local_from_global[p]) ])) for p in parallel_parameters ]) existing_preconditions = [(assigned_predicate, to_constant(p), local_from_global[p]) for p in ancestor_parameters] constant_pairs = [(a, p.name) for a, p in safe_zip(args, new_action.parameters) if is_constant(a)] group_preconditions = [ (group_predicate if is_hashable(a) and (a in constraints.groups) else EQ, to_obj(a), p) for a, p in constant_pairs ] order_preconditions = [ order_facts[idx] for idx in incoming_orders[step] ] new_preconditions = existing_preconditions + group_preconditions + order_preconditions + [ Not(order_facts[step]) ] new_action.precondition = pddl.Conjunction([ new_action.precondition, bound_condition, make_preconditions(new_preconditions) ]).simplified() new_parameters = parameters - ancestors bound_facts = [(bound_predicate, to_constant(p)) for p in new_parameters] assigned_facts = [(assigned_predicate, to_constant(p), local_from_global[p]) for p in new_parameters] new_effects = bound_facts + assigned_facts + [order_facts[step]] new_action.effects.extend(make_effects(new_effects)) # TODO: should also negate the effects of all other sequences here new_actions.append(new_action) #new_action.dump() new_goals.append( And(*[order_facts[idx] for idx in incoming_orders[GOAL_INDEX]])) add_predicate(domain, make_predicate(order_predicate, ['?num', '?step'])) if constraints.exact: domain.actions[:] = [] domain.actions.extend(new_actions) new_goal_exp = And(goal_exp, Or(*new_goals)) for fact in new_facts: add_fact(evaluations, fact, result=INTERNAL_EVALUATION) return new_goal_exp
def fn(outputs, certified): assert (len(outputs) == 1) q0, _, q1 = find_unique(lambda f: f[0] == 'freemotion', certified)[1:] obstacles = fixed + place_movable(certified) free_motion_fn = get_free_motion_gen(robot, obstacles, teleport) return free_motion_fn(q0, q1)
def get_skill(path): return find_unique(path.__contains__, SKILL_COLLECTORS)