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)
