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 planning_from_satisfaction(init, constraints): clusters = cluster_constraints(constraints) prefix = get_internal_prefix(internal=False) assigned_predicate = ASSIGNED_PREDICATE.format(prefix) order_predicate = ORDER_PREDICATE.format(prefix) #order_value_facts = make_order_facts(order_predicate, 0, len(clusters)+1) order_value_facts = [(order_predicate, '_t{}'.format(i)) for i in range(len(clusters) + 1)] init.append(order_value_facts[0]) goal_expression = order_value_facts[-1] order_facts = list(map(obj_from_value_expression, order_value_facts)) bound_parameters = set() actions = [] #constants = {} for i, cluster in enumerate(clusters): objectives = list(map(obj_from_value_expression, cluster.constraints)) constraints, negated, costs = partition_facts(objectives) if negated: raise NotImplementedError(negated) #free_parameters = cluster.parameters - bound_parameters existing_parameters = cluster.parameters & bound_parameters # TODO: confirm that negated predicates work as intended name = 'cluster-{}'.format(i) parameters = list(sorted(cluster.parameters)) preconditions = [(assigned_predicate, to_constant(p), p) for p in sorted(existing_parameters)] + \ constraints + [order_facts[i]] effects = [(assigned_predicate, to_constant(p), p) for p in parameters] + \ [order_facts[i+1], Not(order_facts[i])] if costs: assert len(costs) == 1 [cost] = costs else: cost = None actions.append( make_action(name, parameters, preconditions, effects, cost)) #actions[-1].dump() bound_parameters.update(cluster.parameters) predicates = [make_predicate(order_predicate, ['?step'])] # '?num', domain = make_domain(predicates=predicates, actions=actions) return domain, goal_expression
def constraint_satisfaction(stream_pddl, stream_map, init, terms, stream_info={}, costs=True, max_cost=INF, success_cost=INF, max_time=INF, unit_efforts=False, max_effort=INF, max_skeletons=INF, search_sample_ratio=1, verbose=True, **search_args): # Approaches # 1) Existential quantification of bindings in goal conditions # 2) Backtrack useful streams and then schedule. Create arbitrary outputs for not mentioned. # 3) Construct all useful streams and then associate outputs with bindings # Useful stream must satisfy at least one fact. How should these assignments be propagated though? # Make an action that maps each stream result to unbound values? # TODO: include functions again for cost-sensitive satisfaction # TODO: convert init into streams to bind certain facts # TODO: investigate constraint satisfaction techniques for binding instead # TODO: could also instantiate all possible free parameters even if not useful # TODO: effort that is a function of the number of output parameters (degrees of freedom) # TODO: use a CSP solver instead of a planner internally # TODO: max_iterations? if not terms: return {}, 0, init constraints, negated, functions = partition_facts(set(map(obj_from_existential_expression, terms))) if not costs: functions = [] evaluations = evaluations_from_init(init) goal_facts = set(filter(lambda f: evaluation_from_fact(f) not in evaluations, constraints)) free_parameters = sorted(get_parameters(goal_facts)) print('Parameters:', free_parameters) externals = parse_stream_pddl(stream_pddl, stream_map, stream_info, unit_efforts=unit_efforts) stream_results = extract_streams(evaluations, externals, goal_facts) function_plan = plan_functions(negated + functions, externals) plan_skeleton = [Assignment(free_parameters)] cost = get_optimistic_cost(function_plan) if max_cost < cost: return None, INF, init # TODO: detect connected components # TODO: eagerly evaluate fully bound constraints # TODO: consider other results if this fails domain = create_domain(goal_facts) init_evaluations = evaluations.copy() store = SolutionStore(evaluations, max_time=max_time, success_cost=success_cost, verbose=verbose) queue = SkeletonQueue(store, domain, disable=False) num_iterations = search_time = sample_time = 0 planner = 'ff-astar' # TODO: toggle within reschedule_stream_plan #last_clusters = set() #last_success = True while not store.is_terminated(): num_iterations += 1 start_time = time.time() print('\nIteration: {} | Skeletons: {} | Skeleton Queue: {} | Evaluations: {} | ' 'Cost: {:.3f} | Search Time: {:.3f} | Sample Time: {:.3f} | Total Time: {:.3f}'.format( num_iterations, len(queue.skeletons), len(queue), len(evaluations), store.best_cost, search_time, sample_time, store.elapsed_time())) external_plan = None if len(queue.skeletons) < max_skeletons: domain.axioms[:] = create_disabled_axioms(queue, use_parameters=False) #dominated = are_domainated(last_clusters, clusters) #last_clusters = clusters #if last_success or not dominated: # Could also keep a history of results stream_plan = reschedule_stream_plan(init_evaluations, goal_facts, domain, stream_results, unique_binding=True, unsatisfiable=True, max_effort=max_effort, planner=planner, **search_args) if stream_plan is not None: external_plan = reorder_stream_plan(combine_optimizers( init_evaluations, stream_plan + list(function_plan))) print('Stream plan ({}, {:.3f}): {}'.format( get_length(external_plan), compute_plan_effort(external_plan), external_plan)) last_success = (external_plan is not None) search_time += elapsed_time(start_time) # Once a constraint added for a skeleton, it should only be relaxed start_time = time.time() if last_success: # Only works if create_disable_axioms never changes allocated_sample_time = (search_sample_ratio * search_time) - sample_time else: allocated_sample_time = INF queue.process(external_plan, plan_skeleton, cost=cost, complexity_limit=INF, max_time=allocated_sample_time) sample_time += elapsed_time(start_time) if not last_success and not queue: break # TODO: exhaustively compute all plan skeletons and add to queue within the focused algorithm write_stream_statistics(externals, verbose) action_plan, cost, facts = revert_solution(store.best_plan, store.best_cost, evaluations) bindings = bindings_from_plan(plan_skeleton, action_plan) return bindings, cost, facts
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 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])