def __init__(self, name, info, inputs, domain):
super(External, self).__init__(name, info)
self.inputs = tuple(inputs)
self.domain = tuple(map(convert_constants, domain))
for p, c in Counter(self.inputs).items():
if not is_parameter(p):
# AssertionError: Expected item to be a variable: q2 in (?q1 q2)
raise ValueError(
'Input [{}] for stream [{}] is not a parameter'.format(
p, name))
if c != 1:
raise ValueError(
'Input [{}] for stream [{}] is not unique'.format(p, name))
parameters = {
a
for i in self.domain for a in get_args(i) if is_parameter(a)
}
for p in (parameters - set(self.inputs)):
raise ValueError(
'Parameter [{}] for stream [{}] is not included within inputs'.
format(p, name))
for p in (set(self.inputs) - parameters):
print(
'Warning! Input [{}] for stream [{}] is not covered by a domain condition'
.format(p, name))
self.constants = {
a
for i in self.domain for a in get_args(i) if not is_parameter(a)
}
self.instances = {}
self.num_opt_fns = 0
def effort_fn(*input_values):
parameter_indices = [i for i, value in enumerate(input_values) if is_parameter(value)]
optimizer_indices = [i for i, value in enumerate(input_values) if isinstance(value, OptValue)
if input_values[i].stream.startswith(optimizer_name)]
#if not parameter_indices and not optimizer_indices:
# return INF
return 1
def reuse_facts(problem, certificate, skeleton):
# TODO: repackage streams
# TODO: recover the full axiom + action plan
# TODO: recover the plan preimage annotated with use time
# Some supporting args are quantified out and thus lack some facts
new_facts = []
if skeleton is None:
return new_facts
reuse_objs = set()
for action, args in skeleton:
for arg in args:
if (arg != WILD) and not is_parameter(arg):
reuse_objs.add(hash_or_id(arg))
# The reuse relpose omission is due to the fact that the initial pose was selected
# (which is populated in the initial state)
order_predicate = ORDER_PREDICATE.format('')
domain = parse_domain(problem.domain_pddl)
fluents = get_fluents(domain)
for fact in certificate.preimage_facts:
predicate = get_prefix(fact)
if (predicate in {order_predicate, EQ}) or (predicate in fluents):
# Could technically evaluate functions as well
continue
if all(
isinstance(arg, str) or (hash_or_id(arg) in reuse_objs)
for arg in get_args(fact)):
new_facts.append(fact)
return new_facts
def is_useful_atom(atom, conditions_from_predicate):
# TODO: this is currently a bottleneck. Instantiate for all actions along the plan first? (apply before checking)
if not isinstance(atom, pddl.Atom):
return False
for atom2 in conditions_from_predicate[atom.predicate]:
if all(is_parameter(a2) or (a1 == a2) for a1, a2 in safe_zip(atom.args, atom2.args)):
return True
return False
def __init__(self, name, gen_fn, inputs, domain, outputs, certified, info, fluents=[], is_wild=False):
super(Stream, self).__init__(name, info, inputs, domain)
self.outputs = tuple(outputs)
self.certified = tuple(certified)
self.constants.update(a for i in certified for a in get_args(i) if not is_parameter(a))
for p, c in Counter(self.outputs).items():
if not is_parameter(p):
raise ValueError('Output [{}] for stream [{}] is not a parameter'.format(p, name))
if c != 1:
raise ValueError('Output [{}] for stream [{}] is not unique'.format(p, name))
for p in set(self.inputs) & set(self.outputs):
raise ValueError('Parameter [{}] for stream [{}] is both an input and output'.format(p, name))
certified_parameters = {a for i in certified for a in get_args(i) if is_parameter(a)}
for p in (certified_parameters - set(self.inputs + self.outputs)):
raise ValueError('Parameter [{}] for stream [{}] is not included within outputs'.format(p, name))
for p in (set(self.outputs) - certified_parameters):
print('Warning! Output [{}] for stream [{}] is not covered by a certified condition'.format(p, name))
# TODO: automatically switch to unique if only used once
self.gen_fn = get_debug_gen_fn(self) if gen_fn == DEBUG else gen_fn
self.num_opt_fns = 1 if self.outputs else 0 # Always unique if no outputs
if isinstance(self.info.opt_gen_fn, PartialInputs):
if self.info.opt_gen_fn.unique:
self.num_opt_fns = 0
self.opt_gen_fn = self.info.opt_gen_fn.get_opt_gen_fn(self)
else:
self.opt_gen_fn = self.info.opt_gen_fn
#self.bound_list_fn = None # TODO: generalize to a hierarchical sequence
#self.opt_fns = [get_unique_fn(self), get_shared_fn(self)] # get_unique_fn | get_shared_fn
self.fluents = [] if gen_fn == DEBUG else fluents
if NEGATIVE_BLOCKED:
self.blocked_predicate = '~{}-negative'.format(self.name) # Args are self.inputs
else:
self.blocked_predicate = '~{}'.format(self.name)
self.disabled_instances = []
self.is_wild = is_wild
if self.is_negated():
if self.outputs:
raise ValueError('Negated streams cannot have outputs: {}'.format(self.outputs))
#assert len(self.certified) == 1 # TODO: is it okay to have more than one fact?
for certified in self.certified:
if not (set(self.inputs) <= set(get_args(certified))):
raise ValueError('Negated streams must have certified facts including all input parameters')
def visualize_constraints(constraints,
filename='constraint_network.pdf',
use_functions=True):
from pygraphviz import AGraph
graph = AGraph(strict=True, directed=False)
graph.node_attr['style'] = 'filled'
#graph.node_attr['fontcolor'] = 'black'
#graph.node_attr['fontsize'] = 12
graph.node_attr['colorscheme'] = 'SVG'
graph.edge_attr['colorscheme'] = 'SVG'
#graph.graph_attr['rotate'] = 90
#graph.node_attr['fixedsize'] = True
graph.node_attr['width'] = 0
graph.node_attr['height'] = 0.02 # Minimum height is 0.02
graph.node_attr['margin'] = 0
graph.graph_attr['rankdir'] = 'RL'
graph.graph_attr['nodesep'] = 0.05
graph.graph_attr['ranksep'] = 0.25
#graph.graph_attr['pad'] = 0
# splines="false";
graph.graph_attr['outputMode'] = 'nodesfirst'
graph.graph_attr['dpi'] = 300
functions = set()
negated = set()
heads = set()
for fact in constraints:
prefix = get_prefix(fact)
if prefix in (EQ, MINIMIZE):
functions.add(fact[1])
elif prefix == NOT:
negated.add(fact[1])
else:
heads.add(fact)
heads.update(functions)
heads.update(negated)
for head in heads:
if not use_functions and (head in functions):
continue
# TODO: prune values w/o free parameters?
name = str_from_fact(head)
if head in functions:
color = COST_COLOR
elif head in negated:
color = NEGATED_COLOR
else:
color = CONSTRAINT_COLOR
graph.add_node(name, shape='box', color=color)
for arg in get_args(head):
if isinstance(arg, OptimisticObject) or is_parameter(arg):
arg_name = str(arg)
graph.add_node(arg_name, shape='circle', color=PARAMETER_COLOR)
graph.add_edge(name, arg_name)
graph.draw(filename, prog='dot') # neato | dot | twopi | circo | fdp | nop
return graph
def main(success_cost=0):
parser = argparse.ArgumentParser()
parser.add_argument('-d', '--deterministic', action='store_true', help='Uses a deterministic sampler')
parser.add_argument('-a', '--algorithm', default='', help='Specifies the algorithm')
parser.add_argument('-o', '--optimizer', action='store_true', help='Uses the optimizers')
parser.add_argument('-t', '--max_time', default=30, type=int, help='The max time')
parser.add_argument('-u', '--unit', action='store_true', help='Uses unit costs')
args = parser.parse_args()
print('Arguments:', args)
np.set_printoptions(precision=2)
if args.deterministic:
set_deterministic()
print('Random seed:', get_random_seed())
tamp_problem = get_tight_problem(n_blocks=2, n_goals=2)
print(tamp_problem)
pddlstream_problem = pddlstream_from_tamp(tamp_problem, use_stream=not args.optimizer,
use_optimizer=args.optimizer)
stream_pddl, stream_map = pddlstream_problem[2:4]
stream_info = {
't-region': StreamInfo(eager=True, p_success=0), # bound_fn is None
#'t-cfree': StreamInfo(eager=False, negate=True),
#'distance': FunctionInfo(opt_fn=lambda q1, q2: MOVE_COST), # Doesn't make a difference
}
terms = CONSTRAINTS + OBJECTIVES
pr = cProfile.Profile()
pr.enable()
if args.algorithm == 'focused':
solution = solve_pddlstream_satisfaction(stream_pddl, stream_map, INIT, terms,
incremental=False, stream_info=stream_info,
#search_sample_ratio=1,
#max_skeletons=1,
success_cost=success_cost, max_time=args.max_time)
elif args.algorithm == 'incremental':
solution = solve_pddlstream_satisfaction(stream_pddl, stream_map, INIT, terms, incremental=True,
success_cost=success_cost, max_time=args.max_time,
verbose=False, debug=False)
else:
solution = constraint_satisfaction(stream_pddl, stream_map, INIT, terms, stream_info=stream_info,
costs=not args.unit, success_cost=success_cost,
max_time=args.max_time, search_sample_ratio=1,
debug=False)
#raise ValueError(args.algorithm)
dump_assignment(solution)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
bindings, cost, evaluations = solution
if bindings is None:
return
plan = []
for name, args in SKELETON:
new_args = [bindings[a] if is_parameter(a) else a for a in args]
plan.append((name, new_args))
display_plan(tamp_problem, plan)
def get_necessary_axioms(conditions, axioms, negative_from_name):
if not conditions or not axioms:
return {}
axioms_from_name = get_derived_predicates(axioms)
atom_queue = []
processed_atoms = set()
def add_literals(literals):
for lit in literals:
atom = lit.positive()
if atom not in processed_atoms:
atom_queue.append(
atom) # Previously was lit.positive() for some reason?
processed_atoms.add(atom)
import pddl
add_literals(conditions)
axiom_from_action = {}
partial_instantiations = set()
while atom_queue:
literal = atom_queue.pop()
for axiom in axioms_from_name[literal.predicate]:
derived_parameters = axiom.parameters[:axiom.
num_external_parameters]
var_mapping = {
p.name: a
for p, a in zip(derived_parameters, literal.args)
if not is_parameter(a)
}
key = (axiom, frozenset(var_mapping.items()))
if key in partial_instantiations:
continue
partial_instantiations.add(key)
parts = [
l.rename_variables(var_mapping)
for l in get_literals(axiom.condition)
if l.predicate not in negative_from_name
] # Assumes a conjunction?
# new_condition = axiom.condition.uniquify_variables(None, var_mapping)
effect_args = [
var_mapping.get(a.name, a.name) for a in derived_parameters
]
effect = pddl.Effect([], pddl.Truth(),
pddl.conditions.Atom(axiom.name, effect_args))
free_parameters = [
p for p in axiom.parameters if p.name not in var_mapping
]
new_action = pddl.Action(axiom.name, free_parameters,
len(free_parameters),
pddl.Conjunction(parts), [effect], None)
# Creating actions so I can partially instantiate (impossible with axioms)
axiom_from_action[new_action] = (axiom, var_mapping)
add_literals(parts)
return axiom_from_action
def existential_quantification(goal_literals):
# TODO: merge with pddlstream-experiments
goal_formula = []
for literal in goal_literals:
parameters = [a for a in get_args(literal) if is_parameter(a)]
if parameters:
type_literals = [('Type', p, get_parameter_name(p))
for p in parameters]
goal_formula.append(
Exists(parameters, And(literal, *type_literals)))
else:
goal_formula.append(literal)
return And(*goal_formula)
def extract_function_results(results_from_head, action, pddl_args):
import pddl
if action.cost is None:
return None
expression = action.cost.expression
if not isinstance(expression, pddl.PrimitiveNumericExpression):
return None
var_mapping = {p.name: a for p, a in zip(action.parameters, pddl_args)}
obj_args = tuple(
obj_from_pddl(var_mapping[p] if is_parameter(p) else p)
for p in expression.args)
head = Head(expression.symbol, obj_args)
[(_, result)] = results_from_head[head]
if result is None:
return None
return result
def instantiate_domain(task, prune_static=True):
fluent_predicates = get_fluents(task)
is_static = lambda a: isinstance(a, pddl.Atom) and (a.predicate not in fluent_predicates)
fluent_facts = MockSet(lambda a: not prune_static or not is_static(a))
init_facts = set(task.init)
function_assignments = get_function_assignments(task)
type_to_objects = instantiate.get_objects_by_type(task.objects, task.types)
constants_from_predicate = defaultdict(set)
for action in task.actions + task.axioms:
for atom in filter(is_static, get_literals(get_precondition(action))):
constants = tuple((i, a) for i, a in enumerate(atom.args) if not is_parameter(a))
constants_from_predicate[atom.predicate].add(constants)
predicate_to_atoms = defaultdict(set)
args_from_predicate = defaultdict(set)
for atom in filter(is_static, task.init): # TODO: compute which predicates might involve constants
predicate_to_atoms[atom.predicate].add(atom)
args_from_predicate[atom.predicate].add(atom.args)
for constants in constants_from_predicate[atom.predicate]:
if all(atom.args[i] == o for i, o in constants):
args_from_predicate[atom.predicate, constants].add(atom.args)
instantiated_actions = []
for action in task.actions:
for variable_mapping in instantiate_condition(action, is_static, args_from_predicate):
inst_action = action.instantiate(variable_mapping, init_facts, fluent_facts, type_to_objects,
task.use_min_cost_metric, function_assignments, predicate_to_atoms)
if inst_action:
instantiated_actions.append(inst_action)
instantiated_axioms = []
for axiom in task.axioms:
for variable_mapping in instantiate_condition(axiom, is_static, args_from_predicate):
inst_axiom = axiom.instantiate(variable_mapping, init_facts, fluent_facts)
if inst_axiom:
instantiated_axioms.append(inst_axiom)
reachable_facts, reachable_operators = get_achieving_axioms(init_facts, instantiated_actions + instantiated_axioms)
atoms = {atom for atom in (init_facts | set(reachable_facts)) if isinstance(atom, pddl.Atom)}
relaxed_reachable = all(literal_holds(init_facts, goal) or goal in reachable_facts
for goal in instantiate_goal(task.goal))
reachable_actions = [action for action in reachable_operators
if isinstance(action, pddl.PropositionalAction)]
reachable_axioms = [axiom for axiom in reachable_operators
if isinstance(axiom, pddl.PropositionalAxiom)]
return relaxed_reachable, atoms, reachable_actions, reachable_axioms
def visualize_constraints(constraints,
filename='constraint_network' + DEFAULT_EXTENSION,
use_functions=True):
from pygraphviz import AGraph
graph = AGraph(strict=True, directed=False)
graph.node_attr['style'] = 'filled'
#graph.node_attr['fontcolor'] = 'black'
#graph.node_attr['fontsize'] = 12
graph.node_attr['colorscheme'] = 'SVG'
graph.edge_attr['colorscheme'] = 'SVG'
#graph.graph_attr['rotate'] = 90
#graph.node_attr['fixedsize'] = True
graph.node_attr['width'] = 0
graph.node_attr['height'] = 0.02 # Minimum height is 0.02
graph.node_attr['margin'] = 0
graph.graph_attr['rankdir'] = 'RL'
graph.graph_attr['nodesep'] = 0.05
graph.graph_attr['ranksep'] = 0.25
#graph.graph_attr['pad'] = 0
# splines="false";
graph.graph_attr['outputMode'] = 'nodesfirst'
graph.graph_attr['dpi'] = 300
positive, negated, functions = partition_facts(constraints)
for head in (positive + negated + functions):
# TODO: prune values w/o free parameters?
name = str_from_fact(head)
if head in functions:
if not use_functions:
continue
color = COST_COLOR
elif head in negated:
color = NEGATED_COLOR
else:
color = CONSTRAINT_COLOR
graph.add_node(name, shape='box', color=color)
for arg in get_args(head):
if isinstance(arg, OptimisticObject) or is_parameter(arg):
arg_name = str(arg)
graph.add_node(arg_name, shape='circle', color=PARAMETER_COLOR)
graph.add_edge(name, arg_name)
graph.draw(filename, prog='dot') # neato | dot | twopi | circo | fdp | nop
print('Saved', filename)
return graph
def _add_combinations_relation(self, stream, atoms):
if not all(atoms):
return
# TODO: might be a bug here?
domain = list(map(head_from_fact, stream.domain))
# TODO: compute this first?
relations = [
Relation(filter(is_parameter, domain[index].args), [
tuple(a for a, b in safe_zip(atom.args, domain[index].args)
if is_parameter(b)) for atom in atoms[index]
]) for index in compute_order(domain, atoms)
]
solution = solve_satisfaction(relations)
for element in solution.body:
mapping = solution.get_mapping(element)
input_objects = safe_apply_mapping(stream.inputs, mapping)
self.push_instance(stream.get_instance(input_objects))
def extract_function_result(results_from_head, action, pddl_args):
import pddl
if action.cost is None:
return None
# TODO: retrieve constant action costs
# TODO: associate costs with the steps they are applied
expression = action.cost.expression
if not isinstance(expression, pddl.PrimitiveNumericExpression):
return None
var_mapping = {p.name: a for p, a in zip(action.parameters, pddl_args)}
obj_args = tuple(
obj_from_pddl(var_mapping[p] if is_parameter(p) else p)
for p in expression.args)
head = Head(expression.symbol, obj_args)
[result] = results_from_head[head]
if result is None:
return None
return result
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 get_constants(atom):
return tuple((i, a) for i, a in enumerate(atom.args) if not is_parameter(a))
def is_instance(atom, schema):
return (atom.function == schema.function) and \
all(is_parameter(b) or (a == b)
for a, b in safe_zip(atom.args, schema.args))
def fn(outputs, facts, hint={}):
# TODO: pass in the variables and constraint streams instead?
# The true test is placing two blocks in a tight region obstructed by one
positive, negative, costs = partition_facts(facts)
#print('Parameters:', outputs)
print('Constraints:', positive + negative)
if costs:
print('Costs:', costs)
model = Model(name='TAMP')
model.setParam(GRB.Param.OutputFlag, verbose)
model.setParam(GRB.Param.TimeLimit, max_time)
var_from_param = {}
for fact in facts:
prefix, args = fact[0], fact[1:]
if prefix == 'conf':
param, = args
if is_parameter(param):
var_from_param[param] = np_var(model)
elif prefix == 'pose':
_, param = args
if is_parameter(param):
var_from_param[param] = np_var(model)
elif prefix == 'traj':
raise NotImplementedError()
#param, = args
#if param not in var_from_id:
# var_from_id[id(param)] = [np_var(model), np_var(model)]
def get_var(p):
return var_from_param[p] if is_parameter(p) else p
objective_terms = []
constraint_from_name = {}
for index, fact in enumerate(facts):
prefix, args = fact[0], fact[1:]
name = str(index)
if prefix == 'kin':
kinematics_constraint(model, name, *map(get_var, args))
elif prefix == 'contained':
contained_constraint(model, regions, name, *map(get_var, args))
elif prefix == 'cfree' and collisions:
collision_constraint(model, name, *map(get_var, args))
elif prefix == 'motion':
#motion_constraint(model, name, *map(get_var, args))
raise NotImplementedError()
elif prefix == NOT:
fact = args[0]
predicate, args = fact[0], fact[1:]
if predicate == 'posecollision' and collisions:
collision_constraint(model, name, *map(get_var, args))
elif prefix == MINIMIZE:
fact = args[0]
func, args = fact[0], fact[1:]
if func == 'distance':
objective_terms.extend(distance_cost(*map(get_var, args)))
continue
constraint_from_name[name] = fact
for out, value in hint.items():
for var, coord in zip(get_var(out), value):
var.start = coord
model.setObjective(quicksum(objective_terms), sense=GRB.MINIMIZE)
#m.write("file.lp")
model.optimize()
# https://www.gurobi.com/documentation/7.5/refman/optimization_status_codes.html
if model.status in (GRB.INFEASIBLE,
GRB.INF_OR_UNBD): # OPTIMAL | SUBOPTIMAL
if not diagnose:
return OptimizerOutput()
constraint_indices = {
i
for i, term in enumerate(facts) if term[0] != MINIMIZE
}
#infeasible = constraint_indices
#infeasible = compute_inconsistent(model)
#infeasible = deletion_filter(model, constraint_indices)
infeasible = elastic_filter(model, constraint_indices)
infeasible_facts = [facts[index] for index in sorted(infeasible)]
print('Inconsistent:', infeasible_facts)
return OptimizerOutput(infeasible=[infeasible])
assignment = tuple(value_from_var(get_var(out)) for out in outputs)
return OptimizerOutput(assignments=[assignment])
def get_var(p):
return var_from_param[p] if is_parameter(p) else p
def fn(outputs, facts, hint={}):
# TODO: pass in the variables and constraint streams instead?
# The true test is placing two blocks in a tight region obstructed by one
constraint_indices = {
i
for i, term in enumerate(facts) if term[0] != MINIMIZE
}
positive, negative, costs = partition_facts(facts)
#print('Parameters:', outputs)
#print('Constraints:', positive + negative)
if costs:
print('Costs:', costs)
# https://github.com/yijiangh/coop_assembly/blob/e52abef7c1cfb1d3e32691d163abc85dd77f27a2/src/coop_assembly/geometry_generation/caelan.py
model = Model(name='TAMP')
model.setParam(GRB.Param.OutputFlag, verbose)
model.setParam(GRB.Param.TimeLimit, max_time)
model.setParam(GRB.Param.Cutoff,
GRB.INFINITY) # TODO: account for scaling
#if num_solutions < INF:
# model.setParam(GRB.Param.SolutionLimit, num_solutions)
# Limit how many solutions to collect
#model.setParam(GRB.Param.PoolSolutions, 2)
# Limit the search space by setting a gap for the worst possible solution that will be accepted
#model.setParam(GRB.Param.PoolGap, 0.10) # PoolGapAbs
# do a systematic search for the k-best solutions
#model.setParam(GRB.Param.PoolSearchMode, 2) # 0 | 1 | 2
# https://www.gurobi.com/documentation/9.1/examples/poolsearch_py.html#subsubsection:poolsearch.py
##########
# TODO: remove anything that's just a domain condition?
variable_indices = {}
var_from_param = {}
for index, fact in enumerate(facts):
prefix, args = fact[0], fact[1:]
if prefix == 'conf':
param, = args
if is_parameter(param):
var_from_param[param] = np_var(model,
lower=lower,
upper=upper)
elif prefix == 'pose':
_, param = args
if is_parameter(param):
var_from_param[param] = np_var(model,
lower=lower,
upper=upper)
elif prefix == 'grasp': # TODO: iterate over combinations
_, param = args
if is_parameter(param):
var_from_param[param] = GRASP
elif prefix == 'traj':
raise NotImplementedError()
#param, = args
#if param not in var_from_id:
# var_from_id[id(param)] = [np_var(model), np_var(model)]
else:
continue
variable_indices[index] = fact
dimension = sum(len(var) for var in var_from_param.values())
def get_var(p):
return var_from_param[p] if is_parameter(p) else p
##########
codimension = 0
objective_terms = [
] # TODO: could make a variable to impose a cost constraint
constraint_from_name = {}
for index, fact in enumerate(facts):
prefix, args = fact[0], fact[1:]
name = str(index)
if prefix == 'kin':
kinematics_constraint(model, name, *map(get_var, args))
codimension += 2
elif prefix in ('contain', 'contained'):
contained_constraint(model, regions, name, *map(get_var, args))
codimension += 1
elif prefix == 'cfree' and collisions:
# TODO: drop collision constraints until violated
collision_constraint(model, name, *map(get_var, args))
elif prefix == 'motion':
#motion_constraint(model, name, *map(get_var, args))
raise NotImplementedError()
elif prefix == NOT:
fact = args[0]
predicate, args = fact[0], fact[1:]
if predicate == 'posecollision' and collisions:
collision_constraint(model, name, *map(get_var, args))
elif prefix == MINIMIZE:
fact = args[0]
func, args = fact[0], fact[1:]
if func in ('dist', 'distance'):
objective_terms.extend(
distance_cost(model, *map(get_var, args)))
continue
constraint_from_name[name] = fact
model.update()
##########
#linear_model = model
linear_model = copy_model(model)
#linear_model = Model(name='Linear TAMP')
# TODO: prune linearly dependent constraints
linear_constraints = {
c
for c in linear_model.getConstrs() if c.sense == GRB.EQUAL
}
codimension = len(linear_constraints)
# TODO: account for v.LB == v.UB
#linear_variables = {v for v in linear_model.getVars() if v.VType == GRB.CONTINUOUS}
#print(vars_from_expr(linear_model.getObjective()))
linear_variables = set()
for c in linear_constraints:
linear_variables.update(vars_from_expr(linear_model.getRow(c)))
linear_variables = sorted(linear_variables, key=lambda v: v.VarName)
dimension = len(linear_variables)
print('{} variables (dim={}): {}'.format(
len(variable_indices), dimension,
[facts[index] for index in sorted(variable_indices)]))
nontrivial_indices = set(constraint_indices) - set(
variable_indices) # TODO: rename
print('{} constraints: (codim={}): {}'.format(
len(nontrivial_indices), codimension,
[facts[index] for index in sorted(nontrivial_indices)]))
# # https://en.wikipedia.org/wiki/Linear_subspace
# # TODO: Equations for a subspace
# #for c in model.getConstrs():
# # if c.sense != GRB.EQUAL:
# # model.remove(c)
# variables = [model.getVarByName(v.VarName) for v in linear_variables]
# lower_bound = np.array([v.LB for v in variables])
# upper_bound = np.array([v.UB for v in variables])
# center = (lower_bound + upper_bound) / 2.
# extent = (upper_bound - lower_bound) / 2.
# radius = np.linalg.norm(extent) # sphere
#
# point = radius*sample_sphere(dimension) + center
# #point = center
# basis = [sample_sphere_surface(dimension) for _ in range(codimension)]
# #basis = [np.ones(dimension)]
# multipliers = [unbounded_var(model) for _ in basis]
# subspace_constraints = []
# for i in range(dimension):
# combination = sum([m*b[i] for m, b in zip(multipliers, basis)])
# subspace_constraints.append(model.addConstr(variables[i] - point[i] == combination))
# #for c in subspace_constraints:
# # model.remove(c)
# TODO: generator version
# for v in set(linear_model.getVars()) - linear_variables:
# linear_model.remove(v)
# for c in set(linear_model.getConstrs()) - linear_constraints:
# linear_model.remove(c)
# linear_model.setObjective(quicksum(sample_targets(linear_model, linear_variables)), sense=GRB.MINIMIZE)
# linear_model.optimize()
# for v in linear_variables: # Projection method
# set_value(model.getVarByName(v.VarName), v.X)
##########
# TODO: normalize cost relative to the best cost for a trade-off
# TODO: increasing bound on deterioration in quality
weight = 0
if weight > 0:
primary_variables = {
v
for var in var_from_param.values() for v in var
}
objective_terms.extend(
weight * term
for term in sample_targets(model, primary_variables))
model.setObjective(
quicksum(objective_terms),
sense=GRB.MINIMIZE) # (1-weight) * quicksum(objective_terms)
for out, value in hint.items():
for var, coord in zip(get_var(out), value):
# https://www.gurobi.com/documentation/9.1/refman/varhintval.html#attr:VarHintVal
set_guess(var, coord, hard=hard)
#set_value(var, coord)
##########
#m.write("file.lp")
model.optimize()
# https://www.gurobi.com/documentation/7.5/refman/optimization_status_codes.html
#if model.status in (GRB.INFEASIBLE, GRB.INF_OR_UNBD, GRB.CUTOFF): # OPTIMAL | SUBOPTIMAL
if model.SolCount == 0:
if diagnostic is None:
return OptimizerOutput()
elif diagnostic == 'all':
#infeasible = constraint_indices
infeasible = nontrivial_indices
elif diagnostic == 'deletion':
infeasible = deletion_filter(model, constraint_indices)
elif diagnostic == 'elastic':
infeasible = elastic_filter(model, constraint_indices)
elif diagnostic == 'gurobi':
infeasible = compute_inconsistent(model)
else:
raise NotImplementedError(diagnostic)
print('Inconsistent:',
[facts[index] for index in sorted(infeasible)])
return OptimizerOutput(infeasible=[infeasible])
#expr.getValue() # TODO: store expressions and evaluate value
# for c in model.getConstrs():
# print(c, c.Slack, c.RHS)
# print(c.__dict__)
# print(dir(c))
##########
print(
'Solved: {} | Objective: {:.3f} | Solutions: {} | Status: {} | Runtime: {:.3f}'
.format(True, model.ObjVal, model.SolCount, model.status,
model.runtime))
if costs and diagnose_cost:
infeasible = deletion_filter(model,
constraint_indices,
max_objective=model.ObjVal - 1e-6)
else:
# TODO: propagate automatically to optimizer
#infeasible = constraint_indices
infeasible = nontrivial_indices
print('Cost inconsistent:',
[facts[index] for index in sorted(infeasible)])
# variables = list(var_from_param.values())
# for index, solution in enumerate(sample_solutions(model, variables, num_samples=15)):
# print(index, solution)
assignment = tuple(value_from_var(get_var(out)) for out in outputs)
return OptimizerOutput(assignments=[assignment],
infeasible=[infeasible])
def obj_from_value_expression(parent):
return replace_expression(
parent, lambda o: o if is_parameter(o) else Object.from_value(o))
def pddlstream_from_problem(problem,
base_limits=None,
collisions=True,
teleport=False):
robot = problem.robot
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {
'@sink': 'sink',
'@stove': 'stove',
}
#initial_bq = Pose(robot, get_pose(robot))
initial_bq = Conf(robot, get_group_joints(robot, 'base'),
get_group_conf(robot, 'base'))
init = [
('CanMove',),
('BConf', initial_bq),
('AtBConf', initial_bq),
Equal(('PickCost',), 1),
Equal(('PlaceCost',), 1),
] + [('Sink', s) for s in problem.sinks] + \
[('Stove', s) for s in problem.stoves] + \
[('Connected', b, d) for b, d in problem.buttons] + \
[('Button', b) for b, _ in problem.buttons]
for arm in ARM_NAMES:
#for arm in problem.arms:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf), ('HandEmpty', arm),
('AtAConf', arm, conf)]
if arm in problem.arms:
init += [('Controllable', arm)]
for body in problem.movable:
pose = Pose(body, get_pose(body), init=True) # TODO: supported here
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', body, pose), ('Stackable', body, None)]
for surface in problem.surfaces:
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
for body, ty in problem.body_types:
init += [('Type', body, ty)]
bodies_from_type = get_bodies_from_type(problem)
goal_literals = []
if problem.goal_conf is not None:
goal_conf = Conf(robot, get_group_joints(robot, 'base'),
problem.goal_conf)
init += [('BConf', goal_conf)]
goal_literals += [('AtBConf', goal_conf)]
for ty, s in problem.goal_on:
bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter(
ty) else [ty]
init += [('Stackable', b, s) for b in bodies]
goal_literals += [('On', ty, s)]
goal_literals += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
goal_formula = []
for literal in goal_literals:
parameters = [a for a in get_args(literal) if is_parameter(a)]
if parameters:
type_literals = [('Type', p, get_parameter_name(p))
for p in parameters]
goal_formula.append(
Exists(parameters, And(literal, *type_literals)))
else:
goal_formula.append(literal)
goal_formula = And(*goal_formula)
custom_limits = {}
if base_limits is not None:
custom_limits.update(get_custom_limits(robot, problem.base_limits))
stream_map = {
'sample-pose':
from_gen_fn(get_stable_gen(problem, collisions=collisions)),
'sample-grasp':
from_list_fn(get_grasp_gen(problem, collisions=collisions)),
#'sample-grasp': from_gen_fn(get_grasp_gen(problem, collisions=collisions)),
'inverse-kinematics':
from_gen_fn(
get_ik_ir_gen(problem,
custom_limits=custom_limits,
collisions=collisions,
teleport=teleport)),
'plan-base-motion':
from_fn(
get_motion_gen(problem,
custom_limits=custom_limits,
collisions=collisions,
teleport=teleport)),
'test-cfree-pose-pose':
from_test(get_cfree_pose_pose_test(collisions=collisions)),
'test-cfree-approach-pose':
from_test(get_cfree_approach_pose_test(problem,
collisions=collisions)),
'test-cfree-traj-pose':
from_test(get_cfree_traj_pose_test(robot, collisions=collisions)),
#'test-cfree-traj-grasp-pose': from_test(get_cfree_traj_grasp_pose_test(problem, collisions=collisions)),
#'MoveCost': move_cost_fn,
'Distance':
distance_fn,
}
#stream_map = DEBUG
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal_formula)
def fn(outputs, facts, hint={}):
print(outputs, facts)
model = Model(name='TAMP')
model.setParam(GRB.Param.OutputFlag, verbose)
if max_time < INF:
model.setParam(GRB.Param.TimeLimit, max_time)
var_from_param = {}
for fact in facts:
prefix, args = fact[0], fact[1:]
if prefix in ['wcash', 'pcash', 'mcash']:
cash, = args
if is_parameter(cash):
# TODO: scale by 100 for cents
var_from_param[cash] = model.addVar(
lb=0,
ub=GRB.INFINITY,
vtype=GRB.INTEGER if integer else GRB.CONTINUOUS)
if prefix == 'wcash':
model.addConstr(var_from_param[cash] >= min_take)
if max_take < INF:
model.addConstr(var_from_param[cash] <= max_take)
if (prefix == 'pcash') and (max_wallet < INF):
model.addConstr(var_from_param[cash] <= max_wallet)
if prefix == 'mcash':
# min_balance >= 0
pass
get_var = lambda p: var_from_param[p] if is_parameter(
p) else p # var_from_param.get(p, p)
objective_terms = []
for index, fact in enumerate(facts):
name = str(index)
if fact[0] == MINIMIZE:
fact = fact[1]
func, args = fact[0], map(get_var, fact[1:])
if func == 'withdrawcost':
cash, = args
objective_terms.append(cash)
elif fact[0] == NOT:
fact = fact[1]
predicate, args = fact[0], map(get_var, fact[1:])
else:
prefix, args = fact[0], map(get_var, fact[1:])
if prefix == 'ge':
cash1, cash2 = args
model.addConstr(cash1 >= cash2, name=name)
elif prefix == 'withdraw':
wcash, pcash1, pcash2, mcash1, mcash2 = args
model.addConstr(pcash1 + wcash == pcash2, name=name)
model.addConstr(mcash1 - wcash == mcash2, name=name)
model.setObjective(quicksum(objective_terms), sense=GRB.MINIMIZE)
try:
model.optimize()
except GurobiError as e:
raise e
objective = 0
if objective_terms:
objective = INF if model.status == GRB.INFEASIBLE else model.objVal
print('Objective: {:.3f} | Solutions: {} | Status: {}'.format(
objective, model.solCount, model.status))
# https://www.gurobi.com/documentation/9.0/refman/optimization_status_codes.html
if not model.solCount: # GRB.INFEASIBLE | GRB.INF_OR_UNBD | OPTIMAL | SUBOPTIMAL | UNBOUNDED
return OptimizerOutput()
assignment = tuple(get_var(out).x for out in outputs)
return OptimizerOutput(assignments=[assignment])
def main(success_cost=INF, use_costs=True): # 0 | INF
parser = argparse.ArgumentParser()
parser.add_argument('-d',
'--deterministic',
action='store_true',
help='Uses a deterministic sampler')
parser.add_argument('-a',
'--algorithm',
default='',
help='Specifies the algorithm')
parser.add_argument('-g',
'--gurobi',
action='store_true',
help='Uses gurobi')
parser.add_argument('-t',
'--max_time',
default=30,
type=int,
help='The max time')
parser.add_argument('-u',
'--unit',
action='store_true',
help='Uses unit costs')
args = parser.parse_args()
print('Arguments:', args)
np.set_printoptions(precision=2)
if args.deterministic:
set_deterministic()
print('Random seed:', get_random_seed())
tamp_problem = tight(n_robots=1, n_blocks=2, n_goals=2)
print(tamp_problem)
pddlstream_problem = pddlstream_from_tamp(tamp_problem,
use_stream=not args.gurobi,
use_optimizer=args.gurobi)
_, _, stream_pddl, stream_map, _, _ = pddlstream_problem
stream_info = {
't-region': StreamInfo(eager=True, p_success=0), # bound_fn is None
#'t-cfree': StreamInfo(eager=False, negate=True),
#'distance': FunctionInfo(opt_fn=lambda q1, q2: MOVE_COST), # Doesn't make a difference
}
terms = CONSTRAINTS
print('Constraints:', CONSTRAINTS)
if use_costs:
print('Objectives:', OBJECTIVES)
terms += OBJECTIVES
satisfaction_problem = SatisfactionProblem(stream_pddl, stream_map, INIT,
terms)
with Profiler():
if args.algorithm == 'focused':
solution = solve_pddlstream_satisfaction(
satisfaction_problem,
incremental=False,
stream_info=stream_info,
#search_sample_ratio=1,
#max_skeletons=1,
success_cost=success_cost,
max_time=args.max_time)
elif args.algorithm == 'incremental':
assert not args.gurobi
solution = solve_pddlstream_satisfaction(satisfaction_problem,
incremental=True,
success_cost=success_cost,
max_time=args.max_time,
verbose=False,
debug=False)
else:
solution = constraint_satisfaction(satisfaction_problem,
stream_info=stream_info,
costs=not args.unit,
success_cost=success_cost,
max_time=args.max_time,
search_sample_ratio=1,
debug=False)
#raise ValueError(args.algorithm)
dump_assignment(solution)
bindings, cost, evaluations = solution
if bindings is None:
return
plan = []
for name, args in SKELETON:
new_args = [bindings[a] if is_parameter(a) else a for a in args]
plan.append((name, new_args))
display_plan(tamp_problem, retime_plan(plan))
def parse_value(value):
return OptimisticObject.from_opt(
value, value) if is_parameter(value) else Object.from_value(value)
def __init__(self,
name,
gen_fn,
inputs,
domain,
outputs,
certified,
info=StreamInfo(),
fluents=[]):
super(Stream, self).__init__(name, info, inputs, domain)
self.outputs = tuple(outputs)
self.certified = tuple(map(convert_constants, certified))
self.constants.update(a for i in certified for a in get_args(i)
if not is_parameter(a))
self.fluents = fluents
#self.fluents = [] if (gen_fn in DEBUG_MODES) else fluents
for p, c in Counter(self.outputs).items():
if not is_parameter(p):
raise ValueError(
'Output [{}] for stream [{}] is not a parameter'.format(
p, name))
if c != 1:
raise ValueError(
'Output [{}] for stream [{}] is not unique'.format(
p, name))
for p in set(self.inputs) & set(self.outputs):
raise ValueError(
'Parameter [{}] for stream [{}] is both an input and output'.
format(p, name))
certified_parameters = {
a
for i in certified for a in get_args(i) if is_parameter(a)
}
for p in (certified_parameters - set(self.inputs + self.outputs)):
raise ValueError(
'Parameter [{}] for stream [{}] is not included within outputs'
.format(p, name))
for p in (set(self.outputs) - certified_parameters):
print(
'Warning! Output [{}] for stream [{}] is not covered by a certified condition'
.format(p, name))
# TODO: automatically switch to unique if only used once
self.gen_fn = gen_fn # DEBUG_MODES
if gen_fn == DEBUG:
self.gen_fn = get_debug_gen_fn(
self, shared=False
) # TODO: list of abstractions that is considered in turn
elif gen_fn == SHARED_DEBUG:
self.gen_fn = get_debug_gen_fn(self, shared=True)
assert callable(self.gen_fn)
self.num_opt_fns = 0 if (self.is_test or self.is_special
) else 1 # TODO: is_negated or is_special
if isinstance(self.info.opt_gen_fn, PartialInputs):
#self.info.opt_gen_fn.register(self)
if self.info.opt_gen_fn.unique:
self.num_opt_fns = 0
#self.bound_list_fn = None # TODO: generalize to a hierarchical sequence
#self.opt_fns = [get_unique_fn(self), get_shared_fn(self)] # get_unique_fn | get_shared_fn
if NEGATIVE_BLOCKED:
self.blocked_predicate = '~{}{}'.format(
self.name, NEGATIVE_SUFFIX) # Args are self.inputs
else:
self.blocked_predicate = '~{}'.format(self.name)
self.disabled_instances = [] # For tracking disabled axioms
self.stream_fact = Fact('_{}'.format(name), concatenate(
inputs, outputs)) # TODO: just add to certified?
if self.is_negated:
if self.outputs:
raise ValueError(
'Negated streams cannot have outputs: {}'.format(
self.outputs))
#assert len(self.certified) == 1 # TODO: is it okay to have more than one fact?
for certified in self.certified:
if not (set(self.inputs) <= set(get_args(certified))):
raise ValueError(
'Negated streams must have certified facts including all input parameters'
)
def is_constant(arg):
return not is_parameter(arg) and (arg != WILD)
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 obj_from_existential_expression(parent): # obj_from_value_expression
return replace_expression(parent, lambda o: OptimisticObject
.from_opt(o, o) if is_parameter(o) else Object.from_value(o))
def convert_constants(fact):
# TODO: take the constant map as an input
# TODO: throw an error if undefined
return Fact(get_prefix(fact), [
p if is_parameter(p) else Object.from_name(p) for p in get_args(fact)
])
