def identify_infeasibility(fn, parameters, terms, **kwargs): # TODO: apply to general constraint networks output = fn(parameters, terms, **kwargs) if output: return output active_indices = {i for i, term in enumerate(terms) if term[0] != MINIMIZE} for index in list(active_indices): constraints = [terms[i] for i in active_indices - {index}] output = fn(parameters, constraints, **kwargs) if output: active_indices.remove(index) # TODO: be careful about removing variables infeasible_facts = [terms[index] for index in sorted(active_indices)] print('Inconsistent:', infeasible_facts) return OptimizerOutput(infeasible=[infeasible_facts])
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 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])