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) ])