def get_problem1():
value = 10
constant_map = {
#'c1': value,
}
stream_pddl = None
stream_map = {}
init = [
#('Goal',),
#('A',),
(
'B', ),
('P1', value),
#('P2', value),
]
goal = Or(
('Goal', ),
#Exists([], ('P2', value)),
#Exists(['?p'], ('P2', '?p')),
(
'Unachievable', ),
)
return PDDLProblem(DOMAIN_PDDL, constant_map, stream_pddl, stream_map,
init, goal)
def solve_first_goal(initial_problem, **kwargs):
domain_pddl, constant_map, stream_pddl, stream_map, init, goal_parts = initial_problem
achieved_parts = []
unachieved_parts = []
for task_part in goal_parts:
# TODO: store any stream evaluations (tests) and limit complexity
problem = create_simplified_problem(initial_problem,
new_goal=task_part)
solution = solve_restart(problem, **kwargs)
plan, _, _ = solution
if plan is None:
unachieved_parts.append(task_part)
elif len(plan) == 0:
achieved_parts.append(task_part)
else:
raise RuntimeError(task_part)
# print(achieved_parts)
# print(unachieved_parts)
# TODO: reset to initial state if not achieved
goal_formula = And(*achieved_parts)
if unachieved_parts:
goal_formula = And(Or(*unachieved_parts), goal_formula)
print(solve_all_goals.__name__, goal_formula)
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal_formula)
return solve_restart(problem, **kwargs)
def create_problem(tamp_problem, hand_empty=False, manipulate_cost=1.):
initial = tamp_problem.initial
assert (not initial.holding)
init = [
#('Region', GROUND_NAME),
Equal(('Cost',), manipulate_cost),
Equal((TOTAL_COST,), 0)] + \
[('Stove', r) for r in STOVE_NAMES if r in tamp_problem.regions] + \
[('Placeable', b, r) for b in initial.block_poses.keys()
for r in tamp_problem.regions if (r in ENVIRONMENT_NAMES) or
((b in tamp_problem.goal_cooked) and (r in STOVE_NAMES))]
for b, p in initial.block_poses.items():
init += [
('Block', b),
('Pose', b, p),
('AtPose', b, p),
#('Grasp', b, GRASP),
]
goal_literals = [] + \
[('Cooked', b) for b in tamp_problem.goal_cooked] # Placeable for the stove
for b, r in tamp_problem.goal_regions.items():
if isinstance(r, np.ndarray):
init += [('Pose', b, r)]
goal_literals += [('AtPose', b, r)]
else:
blocks = [b] if isinstance(b, str) else b
regions = [r] if isinstance(r, str) else r
conditions = []
for body, region in product(blocks, regions):
init += [('Region', region), ('Placeable', body, region)]
conditions += [('In', body, region)]
goal_literals.append(Or(*conditions))
for r, q in initial.robot_confs.items():
init += [
('Robot', r),
('CanMove', r),
('Conf', q),
('AtConf', r, q),
('HandEmpty', r),
]
if hand_empty:
goal_literals += [('HandEmpty', r)]
if tamp_problem.goal_conf is not None:
# goal_literals += [('AtConf', tamp_problem.goal_conf)]
goal_literals += [('AtConf', r, q)]
# goal_literals += [Not(('Unsafe',))] # ('HandEmpty',)
goal = And(*goal_literals)
return init, goal
def create_problem(tamp_problem):
initial = tamp_problem.initial
assert (not initial.holding)
init = [
#('Region', GROUND_NAME),
Equal(('Cost',), 0),
Equal((TOTAL_COST,), 0)] + \
[('Stove', r) for r in STOVE_NAMES if r in tamp_problem.regions] + \
[('Placeable', b, r) for b in initial.block_poses.keys()
for r in tamp_problem.regions if (r in ENVIRONMENT_NAMES) or
((b in tamp_problem.goal_cooked) and (r in STOVE_NAMES))]
for b, p in initial.block_poses.items():
init += [
('Block', b),
('Pose', b, p),
('AtPose', b, p),
('Grasp', b, GRASP),
]
goal_literals = [] + \
[('Cooked', b) for b in tamp_problem.goal_cooked] # Placeable for the stove
for b, r in tamp_problem.goal_regions.items():
if isinstance(r, str):
init += [('Region', r), ('Placeable', b, r)]
goal_literals += [('In', b, r)]
elif isinstance(r, list):
for region in r:
init += [('Region', region), ('Placeable', b, region)]
goal_literals.append(Or(*[('In', b, region) for region in r]))
else:
init += [('Pose', b, r)]
goal_literals += [('AtPose', b, r)]
for r, q in initial.robot_confs.items():
init += [
('Robot', r),
('CanMove', r),
('Conf', q),
('AtConf', r, q),
('HandEmpty', r),
]
if tamp_problem.goal_conf is not None:
# goal_literals += [('AtConf', tamp_problem.goal_conf)]
goal_literals += [('AtConf', r, q)]
# goal_literals += [Not(('Unsafe',))] # ('HandEmpty',)
goal = And(*goal_literals)
return init, goal
def get_problem1(n=5):
# TODO: consider even/odd and version with infinite generator
# TODO: replicate the effect where a stream fails
constant_map = {}
stream_map = {
'increment':
from_fn(lambda x: (x + 1, )), # Increment by different amounts
'decrement': from_fn(lambda x: (x - 1, )),
'test-large': from_test(lambda x: n <= x),
'test-small': from_test(lambda x: x <= -n),
}
init = [
('Integer', 0),
]
goal = Or(
('Goal', )
#Exists(['?x'], ('Large', '?x')),
#Exists(['?x'], ('Small', '?x')),
)
# TODO: sort actions when renaming to make deterministic
return PDDLProblem(DOMAIN_PDDL, constant_map, STREAM_PDDL, stream_map,
init, goal)
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