def fact_from_evaluation(evaluation):
fact = Fact(evaluation.head.function, evaluation.head.args)
if is_atom(evaluation):
return fact
elif is_negated_atom(evaluation):
return Not(fact)
return Equal(fact, evaluation.value)
def instantiate_optimizer_axioms(instantiated, domain, results):
# Needed for instantiating axioms before adding stream action effects
# Otherwise, FastDownward will prune these unreachable axioms
# TODO: compute this first and then apply the eager actions
stream_init = {
fd_from_fact(result.stream_fact)
for result in results if isinstance(result, StreamResult)
}
evaluations = list(
map(evaluation_from_fd, stream_init | instantiated.atoms))
temp_domain = make_domain(
predicates=[make_predicate(UNSATISFIABLE, [])],
axioms=[ax for ax in domain.axioms if ax.name == UNSATISFIABLE])
temp_problem = get_problem(evaluations, Not((UNSATISFIABLE, )),
temp_domain)
# TODO: UNSATISFIABLE might be in atoms making the goal always infeasible
with Verbose():
# TODO: the FastDownward instantiation prunes static preconditions
use_fd = False if using_optimizers(results) else FD_INSTANTIATE
new_instantiated = instantiate_task(task_from_domain_problem(
temp_domain, temp_problem),
use_fd=use_fd,
check_infeasible=False,
prune_static=False)
assert new_instantiated is not None
instantiated.axioms.extend(new_instantiated.axioms)
instantiated.atoms.update(new_instantiated.atoms)
def add_unsatisfiable_to_goal(domain, goal_expression, negate_actions=False):
import pddl
add_predicate(domain, make_predicate(UNSATISFIABLE, []))
if negate_actions:
negated_atom = pddl.NegatedAtom(UNSATISFIABLE, tuple())
for action in domain.actions:
if negated_atom not in action.precondition.parts:
action.precondition = pddl.Conjunction(
[action.precondition, negated_atom]).simplified()
return And(goal_expression, Not((UNSATISFIABLE, )))
def add_unsatisfiable_to_goal(domain, goal_expression):
import pddl
from pddlstream.language.optimizer import UNSATISFIABLE
add_predicate(domain, make_predicate(UNSATISFIABLE, []))
negated_atom = pddl.NegatedAtom(UNSATISFIABLE, tuple())
for action in domain.actions:
if negated_atom not in action.precondition.parts:
action.precondition = pddl.Conjunction(
[action.precondition, negated_atom]).simplified()
return And(goal_expression, Not((UNSATISFIABLE, )))
def augment_goal(domain, goal_expression, negate_actions=False):
# TODO: only do this if optimizers are present
#return goal_expression
import pddl
predicate = pddl.predicates.Predicate(UNSATISFIABLE, tuple())
if predicate.name not in domain.predicate_dict:
domain.predicates.append(predicate)
domain.predicate_dict[predicate.name] = predicate
if negate_actions:
negated_atom = pddl.NegatedAtom(UNSATISFIABLE, tuple())
for action in domain.actions:
if negated_atom not in action.precondition.parts:
action.precondition = pddl.Conjunction(
[action.precondition, negated_atom]).simplified()
return And(goal_expression, Not((UNSATISFIABLE, )))
def planning_from_satisfaction(init, constraints):
clusters = cluster_constraints(constraints)
prefix = get_internal_prefix(internal=False)
assigned_predicate = ASSIGNED_PREDICATE.format(prefix)
order_predicate = ORDER_PREDICATE.format(prefix)
#order_value_facts = make_order_facts(order_predicate, 0, len(clusters)+1)
order_value_facts = [(order_predicate, '_t{}'.format(i))
for i in range(len(clusters) + 1)]
init.append(order_value_facts[0])
goal_expression = order_value_facts[-1]
order_facts = list(map(obj_from_value_expression, order_value_facts))
bound_parameters = set()
actions = []
#constants = {}
for i, cluster in enumerate(clusters):
objectives = list(map(obj_from_value_expression, cluster.constraints))
constraints, negated, costs = partition_facts(objectives)
if negated:
raise NotImplementedError(negated)
#free_parameters = cluster.parameters - bound_parameters
existing_parameters = cluster.parameters & bound_parameters
# TODO: confirm that negated predicates work as intended
name = 'cluster-{}'.format(i)
parameters = list(sorted(cluster.parameters))
preconditions = [(assigned_predicate, to_constant(p), p) for p in sorted(existing_parameters)] + \
constraints + [order_facts[i]]
effects = [(assigned_predicate, to_constant(p), p) for p in parameters] + \
[order_facts[i+1], Not(order_facts[i])]
if costs:
assert len(costs) == 1
[cost] = costs
else:
cost = None
actions.append(
make_action(name, parameters, preconditions, effects, cost))
#actions[-1].dump()
bound_parameters.update(cluster.parameters)
predicates = [make_predicate(order_predicate, ['?step'])] # '?num',
domain = make_domain(predicates=predicates, actions=actions)
return domain, goal_expression
def planning_from_satisfaction(init, constraints):
clusters = cluster_constraints(constraints)
order_value_facts = [(ORDER_PREDICATE, 't{}'.format(i))
for i in range(len(clusters) + 1)]
init.append(order_value_facts[0])
goal_expression = order_value_facts[-1]
order_facts = list(map(obj_from_value_expression, order_value_facts))
bound_parameters = set()
actions = []
#constants = {}
for i, cluster in enumerate(clusters):
objectives = list(map(obj_from_value_expression, cluster.constraints))
#free_parameters = cluster.parameters - bound_parameters
existing_parameters = cluster.parameters & bound_parameters
# TODO: confirm that negated predicates work as intended
name = 'cluster-{}'.format(i)
parameters = list(sorted(cluster.parameters))
preconditions = [(ASSIGNED_PREDICATE, to_constant(p), p) for p in sorted(existing_parameters)] + \
get_constraints(objectives) + [order_facts[i]]
effects = [(ASSIGNED_PREDICATE, to_constant(p), p) for p in parameters] + \
[order_facts[i+1], Not(order_facts[i])]
costs = get_costs(objectives)
cost = None
if costs:
assert len(costs) == 1
cost = get_args(costs[0])[0]
actions.append(
make_action(name, parameters, preconditions, effects, cost))
#actions[-1].dump()
bound_parameters.update(cluster.parameters)
predicates = [make_predicate(ORDER_PREDICATE, ['?x'])]
domain = make_domain(predicates=predicates, actions=actions)
return domain, goal_expression
def get_certified(self):
# TODO: cache these results
expression = self.instance.get_head()
return [expression if self.value else Not(expression)]
def fact_from_fd(fd):
assert (isinstance(fd, pddl.Literal))
atom = (fd.predicate, ) + tuple(map(obj_from_pddl, fd.args))
return Not(atom) if fd.negated else atom
def enforce_single_binding(result, preconditions, effects):
binding_facts = [(BOUND_PREDICATE, pddl_from_object(out))
for out in result.output_objects]
preconditions.extend(Not(fact) for fact in binding_facts)
effects.extend(fact for fact in binding_facts)
def get_certified(self):
expression = self.instance.get_head()
if self.value:
return [expression]
return [Not(expression)]
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 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 get_goal(belief,
init,
base_threshold=(0.05, 0.05, math.radians(10)),
arm_threshold=math.radians(10)):
# TODO: order goals for serialization
# TODO: make independent of belief and world
world = belief.world # One world per state
task = world.task # One task per world
init_bq = belief.base_conf
init_aq = belief.arm_conf
carry_aq = world.carry_conf
goal_literals = [Not(('Unsafe', ))]
if task.goal_hand_empty:
goal_literals.append(('HandEmpty', ))
if task.goal_holding is not None:
goal_literals.append(('Holding', task.goal_holding))
goal_literals += [('On', name, surface) for name, surface in task.goal_on.items()] + \
[Not(('Pressed', name)) for name in KNOBS] + \
[('HasLiquid', cup, liquid) for cup, liquid in task.goal_liquid] + \
[('Cooked', name) for name in task.goal_cooked] + \
[('Localized', name) for name in task.goal_detected] + \
[door_closed_formula(joint_name) for joint_name in task.goal_closed] + \
[door_open_formula(joint_name) for joint_name in task.goal_open] + \
list(task.goal)
if not task.movable_base or task.return_init_bq: # fixed_base?
goal_bq = world.goal_bq if task.movable_base else init_bq
init.extend([
('BConf', goal_bq),
('AConf', goal_bq, carry_aq),
('CloseTo', goal_bq, goal_bq),
])
base_difference_fn = get_difference_fn(world.robot, world.base_joints)
if np.less_equal(
np.abs(base_difference_fn(init_bq.values, goal_bq.values)),
base_threshold).all():
print('Close to goal base configuration')
init.append(('CloseTo', init_bq, goal_bq))
goal_literals.append(
Exists(['?bq'], And(('CloseTo', '?bq', goal_bq),
('AtBConf', '?bq'))))
goal_aq = task.goal_aq
if task.return_init_aq:
goal_aq = init_aq if are_confs_close(
init_aq, world.goal_aq) else world.goal_aq
if goal_aq is not None:
arm_difference_fn = get_difference_fn(world.robot,
world.arm_joints)
if np.less_equal(
np.abs(arm_difference_fn(init_aq.values, goal_aq.values)),
arm_threshold * np.ones(len(world.arm_joints))).all():
print('Close to goal arm configuration')
init.append(('CloseTo', init_aq, goal_aq))
init.extend([
('AConf', goal_bq, goal_aq),
('CloseTo', goal_aq, goal_aq),
])
goal_literals.append(
Exists(['?aq'],
And(('CloseTo', '?aq', goal_aq), ('AtAConf', '?aq'))))
return existential_quantification(goal_literals)
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__, 'optimizer1.pddl')) # optimizer1 | optimizer2
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 list(tamp_problem.goal_regions.values()) + [GROUND_NAME]]
goal_literals = [Not(('Unsafe', ))] # ('HandEmpty',)
goal_literals += [('In', b, r)
for b, r in tamp_problem.goal_regions.items()]
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:
# 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)
