def generate_angle_milp(gurobi_model: grb.Model, input, sin_cos_table: List[Tuple]): """MILP method input: theta, thetadot output: thetadotdot, xdotdot (edited) l_{theta, i}, l_{thatdot,i}, l_{thetadotdot, i}, l_{xdotdot, i}, u_.... sum_{i=1}^k l_{x,i} - l_{x,i}*z_i <= x <= sum_{i=1}^k u_{x,i} - u_{x,i}*z_i, for each variable x sum_{i=1}^k l_{theta,i} - l_{theta,i}*z_i <= theta <= sum_{i=1}^k u_{theta,i} - u_{theta,i}*z_i """ theta = input[2] theta_dot = input[3] k = len(sin_cos_table) zs = [] thetaacc = gurobi_model.addMVar(shape=(1,), lb=float("-inf"), name="thetaacc") xacc = gurobi_model.addMVar(shape=(1,), lb=float("-inf"), name="xacc") for i in range(k): z = gurobi_model.addMVar(lb=0, ub=1, shape=(1,), vtype=grb.GRB.INTEGER, name=f"part_{i}") zs.append(z) gurobi_model.addConstr(k - 1 == sum(zs), name=f"const_milp1") theta_lb = 0 theta_ub = 0 theta_dot_lb = 0 theta_dot_ub = 0 thetaacc_lb = 0 thetaacc_ub = 0 xacc_lb = 0 xacc_ub = 0 for i in range(k): theta_interval, theta_dot_interval, theta_acc_interval, xacc_interval = sin_cos_table[i] theta_lb += theta_interval[0].inf - theta_interval[0].inf * zs[i] theta_ub += theta_interval[0].sup - theta_interval[0].sup * zs[i] theta_dot_lb += theta_dot_interval[0].inf - theta_dot_interval[0].inf * zs[i] theta_dot_ub += theta_dot_interval[0].sup - theta_dot_interval[0].sup * zs[i] thetaacc_lb += theta_acc_interval[0].inf - theta_acc_interval[0].inf * zs[i] thetaacc_ub += theta_acc_interval[0].sup - theta_acc_interval[0].sup * zs[i] xacc_lb += xacc_interval[0].inf - xacc_interval[0].inf * zs[i] xacc_ub += xacc_interval[0].sup - xacc_interval[0].sup * zs[i] gurobi_model.addConstr(theta >= theta_lb, name=f"theta_guard1") gurobi_model.addConstr(theta <= theta_ub, name=f"theta_guard2") gurobi_model.addConstr(theta_dot >= theta_dot_lb, name=f"theta_dot_guard1") gurobi_model.addConstr(theta_dot <= theta_dot_ub, name=f"theta_dot_guard2") gurobi_model.addConstr(thetaacc >= thetaacc_lb, name=f"thetaacc_guard1") gurobi_model.addConstr(thetaacc <= thetaacc_ub, name=f"thetaacc_guard2") gurobi_model.addConstr(xacc >= xacc_lb, name=f"xacc_guard1") gurobi_model.addConstr(xacc <= xacc_ub, name=f"xacc_guard2") gurobi_model.update() gurobi_model.optimize() # assert gurobi_model.status == 2, "LP wasn't optimally solved" return thetaacc, xacc
def optimise(self, templates: np.ndarray, gurobi_model: grb.Model, x_prime: tuple): results = [] for template in templates: gurobi_model.update() gurobi_model.setObjective( sum((template[i] * x_prime[i]) for i in range(self.env_input_size)), grb.GRB.MAXIMIZE) gurobi_model.optimize() # print_model(gurobi_model) if gurobi_model.status != 2: return None result = gurobi_model.ObjVal results.append(result) return np.array(results)
def optimise(templates: np.ndarray, gurobi_model: grb.Model, x_prime: tuple): results = [] for template in templates: gurobi_model.update() gurobi_model.setObjective(sum((template[i] * x_prime[i]) for i in range(len(template))), grb.GRB.MAXIMIZE) gurobi_model.optimize() # print_model(gurobi_model) if gurobi_model.status == 5: result = float("inf") results.append(result) continue if gurobi_model.status == 4 or gurobi_model.status == 3: return None assert gurobi_model.status == 2, f"gurobi_model.status=={gurobi_model.status}" # if gurobi_model.status != 2: # return None result = gurobi_model.ObjVal results.append(result) return np.array(results)
def generate_nn_guard(gurobi_model: grb.Model, input, nn: torch.nn.Sequential, action_ego=0, M=1e6): gurobi_vars = [] gurobi_vars.append(input) for i, layer in enumerate(nn): # print(layer) if type(layer) is torch.nn.Linear: v = gurobi_model.addMVar(lb=float("-inf"), shape=(layer.out_features), name=f"layer_{i}") weights: np.ndarray = layer.weight.data.numpy() weights.round(6) lin_expr = weights @ gurobi_vars[-1] if layer.bias is not None: lin_expr = lin_expr + layer.bias.data.numpy() gurobi_model.addConstr(v == lin_expr, name=f"linear_constr_{i}") gurobi_vars.append(v) elif type(layer) is torch.nn.ReLU: v = gurobi_model.addMVar( lb=float("-inf"), shape=gurobi_vars[-1].shape, name=f"layer_{i}") # same shape as previous z = gurobi_model.addMVar(lb=0, ub=1, shape=gurobi_vars[-1].shape, vtype=grb.GRB.INTEGER, name=f"relu_{i}") eps = 0 # 1e-9 # gurobi_model.addConstr(v == grb.max_(0, gurobi_vars[-1])) gurobi_model.addConstr(v >= gurobi_vars[-1], name=f"relu_constr_1_{i}") gurobi_model.addConstr(v <= eps + gurobi_vars[-1] + M * z, name=f"relu_constr_2_{i}") gurobi_model.addConstr(v >= 0, name=f"relu_constr_3_{i}") gurobi_model.addConstr(v <= eps + M - M * z, name=f"relu_constr_4_{i}") gurobi_vars.append(v) # gurobi_model.update() # gurobi_model.optimize() # assert gurobi_model.status == 2, "LP wasn't optimally solved" """ y = Relu(x) 0 <= z <= 1, z is integer y >= x y <= x + Mz y >= 0 y <= M - Mz""" # gurobi_model.update() # gurobi_model.optimize() # assert gurobi_model.status == 2, "LP wasn't optimally solved" # gurobi_model.setObjective(v[action_ego].sum(), grb.GRB.MAXIMIZE) # maximise the output last_layer = gurobi_vars[-1] if action_ego == 0: gurobi_model.addConstr(last_layer[0] >= last_layer[1], name="last_layer") else: gurobi_model.addConstr(last_layer[1] >= last_layer[0], name="last_layer") gurobi_model.update() gurobi_model.optimize() # assert gurobi_model.status == 2, f"LP wasn't optimally solved, gurobi status {gurobi_model.status}" return gurobi_model.status == 2
class LPKnapsackGurobi(SolverDO): def __init__(self, knapsack_model: KnapsackModel, params_objective_function: ParamsObjectiveFunction = None): self.knapsack_model = knapsack_model self.model = None self.variable_decision = {} self.constraints_dict = {} self.description_variable_description = {} self.description_constraint = {} self.aggreg_sol, self.aggreg_dict, self.params_objective_function = \ build_aggreg_function_and_params_objective(problem=self.knapsack_model, params_objective_function=params_objective_function) def init_model(self, **args): warm_start = args.get('warm_start', {}) self.model = Model("Knapsack") self.variable_decision = {"x": {}} self.description_variable_description = { "x": { "shape": self.knapsack_model.nb_items, "type": bool, "descr": "dictionary with key the item index \ and value the boolean value corresponding \ to taking the item or not" } } self.description_constraint["weight"] = { "descr": "sum of weight of used items doesn't exceed max capacity" } weight = {} list_item = self.knapsack_model.list_items max_capacity = self.knapsack_model.max_capacity x = {} for item in list_item: i = item.index x[i] = self.model.addVar(vtype=GRB.BINARY, obj=item.value, name="x_" + str(i)) if i in warm_start: x[i].start = warm_start[i] x[i].varhinstval = warm_start[i] weight[i] = item.weight self.variable_decision["x"] = x self.model.update() self.constraints_dict["weight"] = self.model.addConstr( quicksum([weight[i] * x[i] for i in x]) <= max_capacity) self.model.update() self.model.setParam("TimeLimit", 200) self.model.modelSense = GRB.MAXIMIZE self.model.setParam(GRB.Param.PoolSolutions, 10000) self.model.setParam("MIPGapAbs", 0.00001) self.model.setParam("MIPGap", 0.00000001) def retrieve_solutions(self, range_solutions: Iterable[int]): # nObjectives = S.NumObj solutions = [] fits = [] # x = S.getVars() for s in range_solutions: weight = 0 xs = {} self.model.params.SolutionNumber = s obj = self.model.getAttr("ObjVal") for e in self.variable_decision["x"]: value = self.variable_decision["x"][e].getAttr('Xn') if value <= 0.1: xs[e] = 0 continue xs[e] = 1 weight += self.knapsack_model.index_to_item[e].weight solutions += [ KnapsackSolution(problem=self.knapsack_model, value=obj, weight=weight, list_taken=[xs[e] for e in sorted(xs)]) ] fits += [self.aggreg_sol(solutions[-1])] return ResultStorage( list_solution_fits=[(s, f) for s, f in zip(solutions, fits)], mode_optim=self.params_objective_function.sense_function) def solve(self, parameter_gurobi: ParametersMilp): self.model.setParam("TimeLimit", parameter_gurobi.TimeLimit) self.model.modelSense = GRB.MAXIMIZE self.model.setParam(GRB.Param.PoolSolutions, parameter_gurobi.PoolSolutions) self.model.setParam("MIPGapAbs", parameter_gurobi.MIPGapAbs) self.model.setParam("MIPGap", parameter_gurobi.MIPGap) print("optimizing...") self.model.optimize() nSolutions = self.model.SolCount nObjectives = self.model.NumObj objective = self.model.getObjective().getValue() print('Problem has', nObjectives, 'objectives') print('Gurobi found', nSolutions, 'solutions') if parameter_gurobi.retrieve_all_solution: solutions = self.retrieve_solutions(list(range(nSolutions))) else: solutions = self.retrieve_solutions([0]) return solutions def solve_lns(self, parameter_gurobi: ParametersMilp, init_solution: KnapsackSolution, fraction_decision_fixed: float, nb_iteration_max: int): self.model.setParam("TimeLimit", parameter_gurobi.TimeLimit) self.model.setParam("OutputFlag", 0) self.model.modelSense = GRB.MAXIMIZE self.model.setParam(GRB.Param.PoolSolutions, parameter_gurobi.PoolSolutions) self.model.setParam("MIPGapAbs", parameter_gurobi.MIPGapAbs) self.model.setParam("MIPGap", parameter_gurobi.MIPGap) current_solution = init_solution constraints = {} list_solutions = [current_solution] list_objective = [current_solution.value] objective = init_solution.value for k in trange(nb_iteration_max): for c in constraints: self.model.remove(constraints[c]) self.add_init_solution(current_solution) fixed_variable = set( random.sample( self.variable_decision["x"].keys(), int(fraction_decision_fixed * len(self.variable_decision["x"])))) constraints = self.fix_decision(current_solution, fixed_variable) self.model.optimize() nSolutions = self.model.SolCount nObjectives = self.model.NumObj objective = self.model.getObjective().getValue() if parameter_gurobi.retrieve_all_solution: solutions = self.retrieve_solutions(list(range(nSolutions))) else: solutions = self.retrieve_solutions([0]) current_solution = solutions[0] list_solutions += [solutions[0]] list_objective += [solutions[0].value] print("Last obj : ", list_objective[-1]) fig, ax = plt.subplots(1) ax.plot(list_objective) plt.show() def add_init_solution(self, init_solution: KnapsackSolution): for i in self.variable_decision["x"]: self.variable_decision["x"][i].start = init_solution.list_taken[i] self.variable_decision["x"][ i].varhintval = init_solution.list_taken[i] def fix_decision(self, init_solution: KnapsackSolution, fixed_variable_keys): constraints = {} for i in fixed_variable_keys: constraints[i] = self.model.addConstr( self.variable_decision["x"][i] == init_solution.list_taken[i]) return constraints def describe_the_model(self): return str(self.description_variable_description) + "\n" + str( self.description_constraint)
def more_actors_gurobi(data, n_actors, constraints, first_run=False): """Multi-actor labeling using confidence weighted screen space distance. Args: first_run (bool): Short first run for vis only. n_actors (int): How many actors to label. constraints (Dict[str, Dict[int, int]]): {frame_str => {pose_id => actor_id}}. first_run (bool): Is this the very first run (limit runtime for only vis). Returns: pose_ids (Dict[int, Dict[int, int]]): {frame_id => {actor_id => pose_id}} problem (ActorProblem2): Labeling problem. data (PosesWrapper): Wrapped data for visualization. """ # color_norm = cmNormalize(vmin=0, vmax=n_actors+1) # scalar_map = cm.ScalarMappable(norm=color_norm, cmap='gist_earth') # colors = [tuple(c * 255. for c in scalar_map.to_rgba(i+1)) # for i in range(n_actors)] # print(colors) # raise RuntimeError("") if isinstance(data, Skeleton): data = SkeletonPosesWrapper(skeleton=data) else: assert isinstance(data, dict), "%s" % type(data) data = DataPosesWrapper(data=data) is_conf_normalized = data.is_confidence_normalized() m = Model('Stealth actors') w_unary = 1. # positive unary is a bonus pose_not_present_cost = w_unary * -1000 # negative unary is a penalty problem = ActorProblem2(n_actors=n_actors, pose_not_present_unary_cost=pose_not_present_cost, # positive pairwise is a penalty pose_not_present_pw_cost=1000. * w_unary) objective = None prev_pose_in_2d = None prev_frame_id = None for frame_id in data.get_frames(): # try: # frame_id = int(frame_str.split('_')[1]) # except ValueError: # print("skipping key %s" % frame_id) # continue frame_str = "color_%05d" % frame_id # if frame_id > 30: # break # pose_in = np.array(data[frame_str][u'centered_3d']) # pose_in_2d = np.array(data[frame_str][u'pose_2d']) pose_in_2d = data.get_poses_2d(frame_id=frame_id) # visible = np.array(data[frame_str][u'visible']) # vis_f = np.array(data[frame_str][u'visible_float']) vis_f = data.get_confidences(frame_id=frame_id) assert pose_in_2d.ndim == 3, "no: %s" % repr(pose_in_2d.shape) problem.add_frame(frame_id, n_vars=pose_in_2d.shape[0]) # unary min_count = 0 for pose_id in range(pose_in_2d.shape[0]): conf = vis_f[pose_id, ...] if not is_conf_normalized: conf = get_conf_thresholded(conf, thresh_log_conf=None, dtype_np=np.float32) cnt = np.sum(conf > 0.5) if cnt > conf.shape[0] // 2: min_count += 1 unary = w_unary * np.sum(conf) / conf.shape[0] if frame_id == 251: print("here") # print("[%s] unary: %s" % (frame_id, unary)) problem.add_unary(frame_id, pose_id, cost=unary) problem.set_min_count(frame_id, min_count) # pairwise if prev_pose_in_2d is not None: for prev_pose_id in range(prev_pose_in_2d.shape[0]): prev_pose = prev_pose_in_2d[prev_pose_id, :, :] for pose_id in range(pose_in_2d.shape[0]): pose = pose_in_2d[pose_id, :, :] dist = prev_pose - pose dist = np.linalg.norm(dist, axis=1) dist *= prev_vis_f[prev_pose_id, ...] * vis_f[pose_id, ...] # lg.debug("dist: %s" % repr(dist.shape)) cost = np.sum(dist, axis=0) # if cost > 200: # cost = 1e4 # cost /= 1500 # lg.debug("cost: %s" % cost) problem.add_pw_cost(prev_frame_id, prev_pose_id, frame_id, pose_id, cost) prev_pose_in_2d = pose_in_2d prev_vis_f = vis_f prev_frame_id = frame_id gb_vars = m.addVars(problem.get_n_vars(), vtype=GRB.BINARY) # for lin_id in range(problem.get_n_vars()): # gb_vars[lin_id].set(problem.get_init_for_lin_id(lin_id)) # unary: we want to maximize confidence for lin_id, cost in problem._unary.items(): objective -= gb_vars[lin_id] * cost # pairwise for (lin_id0, lin_id1), cost in problem._pw.items(): objective += gb_vars[lin_id0] * gb_vars[lin_id1] * cost # print("NO PAIRWISE!!!") # a pose can only be labelled once per frame, either # actor0, or actor1, etc. for (frame_id, pose_id), lin_ids in problem._constr_p.items(): constr = None for lin_id in lin_ids: if constr is None: constr = gb_vars[lin_id] else: constr += gb_vars[lin_id] # lg.debug("[%d] pose %d can only be %s" % (frame_id, pose_id, lin_ids)) m.addConstr(constr <= 1) # an actor can only be used once per frame, either # pose0, or pose1, etc. Note: last actor can be used multiple times, # it's the "pose not present" label. for (frame_id, actor_id), lin_ids in problem._constr_a.items(): constr = None for lin_id in lin_ids: if constr is None: constr = gb_vars[lin_id] else: constr += gb_vars[lin_id] # lg.debug("[%d] actor %d can only be %s" # % (frame_id, actor_id, lin_ids)) m.addConstr(constr == 1) # maximum number of poses chosen to be visible <= n_actors # for frame_id, lin_ids in problem._constr_f.items(): # constr = None # for lin_id in lin_ids: # if constr is None: # constr = gb_vars[lin_id] # else: # constr += gb_vars[lin_id] # m.addConstr(constr <= problem._n_actors) first_constrained = False # type: bool min_frame_id = min(data.get_frames()) # type: int assert isinstance(min_frame_id, int) # anchor first pose as first actor if constraints and 'labels' in constraints: for frame_str, labels in constraints['labels'].items(): frame_id = int(frame_str.split('_')[1]) if isinstance(labels, list): assert len(labels) == n_actors, \ "frame: %d, %s %s" % (frame_id, len(labels), n_actors) # assert len(set(labels)) == len(labels), \ # "%s: %s" % (set(labels), labels) labels = {i: v for i, v in enumerate(labels)} for actor_id, pose_id in labels.items(): pose_id = int(pose_id) if pose_id < 0: pose_id = problem._max_pose_ids[frame_id] lin_id = problem.get_lin_id(frame_id, pose_id, actor_id) m.addConstr(gb_vars[lin_id] == 1) if not first_constrained and frame_id == min_frame_id \ and actor_id == 0: first_constrained = True if not first_constrained: m.addConstr(gb_vars[0] == 1) # m.addConstr(gb_vars[36] == 1) # m.addConstr(gb_vars[40] == 1) m.setObjective(objective, GRB.MINIMIZE) m.Params.timeLimit = 300 if not first_run else 10 # m.solver.callSolver(m) m.optimize() pose_ids = defaultdict(dict) prev_frame_id = None prev_lin_ids = {} curr_lin_ids = {} labelings = defaultdict(dict) for lin_id, v in enumerate(m.getVars()): frame_id, pose_id, actor_id = \ problem.get_frame_id_pose_id_actor_id(lin_id) # print("[%d] %s: %s; pose %d is %sactor %s" # % (frame_id, v.varName, v.x, pose_id, # "not " if v.x < 0.5 else "", actor_id)) problem._solution[lin_id] = v.x if prev_frame_id is not None: if prev_frame_id != frame_id: prev_lin_ids = copy.deepcopy(curr_lin_ids) curr_lin_ids.clear() # print("[#{f:d}][{l:d}] unary for p{p0:d}, a{a0:d} is {" # "cost:f}{chosen:s}".format( # f=frame_id, p0=pose_id, a0=actor_id, # cost=problem._unary[lin_id], l=lin_id, # chosen=" <-- chosen" if v.x > 0.5 else "" # )) if v.x > 0.5: curr_lin_ids[lin_id] = {"frame_id": frame_id, "pose_id": pose_id, "actor_id": actor_id} if pose_id == problem._max_pose_ids[frame_id]: pose_id = -1 if frame_id in pose_ids and actor_id != n_actors: assert actor_id not in pose_ids[frame_id], "no" try: pose_ids[frame_id][actor_id] = pose_id except KeyError: pose_ids[frame_id] = {actor_id: pose_id} labelings[frame_id][pose_id] = actor_id # print("pw: %s" % problem._pw[lin_id]) # for lin_id0, entries0 in prev_lin_ids.items(): # if (lin_id0, lin_id) in problem._pw: # print("[#{f:d}] pw {l0:d}(p{p0:d},a{a0:d})" # "->{l1:d}(p{p1:d},a{a1:d}) is {cost:f}".format( # l0=lin_id0, l1=lin_id, # cost=problem._pw[(lin_id0, lin_id)], # a0=entries0['actor_id'], a1=actor_id, # f=frame_id, p0=entries0['pose_id'], p1=pose_id # )) prev_frame_id = frame_id # enforce constraints # if constraints and 'labels' in constraints: # for frame_str, labels in constraints['labels'].items(): # frame_id = int(frame_str.split('_')[1]) # if isinstance(labels, list): # labels = {v: i for i, v in enumerate(labels)} # for pose_id, actor_id in labels.items(): # pose_ids[frame_id][actor_id] = int(pose_id) try: for frame_id in labelings: if frame_id % 5: continue print("\"color_%05d\": {%s}," % (frame_id, ", ".join(["\"%s\": %s" % (key, val) for key, val in labelings[frame_id].items()]))) except TypeError: pass # if we have more, pick the first... # if len(pose_in.shape) > 2: # pose_in = pose_in[0, :, :] # pose_in_2d = pose_in_2d[0, :, :] # visible = visible[0] return pose_ids, problem, data
def identify_actors(data): m = Model('Stealth actors') problem = ActorProblem() objective = None prev_pose_in_2d = None prev_frame_id = None for frame_str in sorted(data): try: frame_id = int(frame_str.split('_')[1]) except ValueError: print("skipping key %s" % frame_id) continue pose_in = np.array(data[frame_str][u'centered_3d']) pose_in_2d = np.array(data[frame_str][u'pose_2d']) visible = np.array(data[frame_str][u'visible']) assert pose_in_2d.ndim == 3, "no: %s" % repr(pose_in_2d.shape) problem.add_frame(frame_id, pose_in_2d.shape[0]) if prev_pose_in_2d is not None: for prev_pose_id in range(prev_pose_in_2d.shape[0]): prev_pose = prev_pose_in_2d[prev_pose_id, :, :] for pose_id in range(pose_in_2d.shape[0]): pose = pose_in_2d[pose_id, :, :] dist = prev_pose - pose lg.debug("dist: %s" % repr(dist.shape)) cost = np.sum(np.linalg.norm(dist, axis=1), axis=0) lg.debug("cost: %s" % cost) problem.add_cost(prev_frame_id, prev_pose_id, frame_id, pose_id, cost) prev_pose_in_2d = pose_in_2d prev_frame_id = frame_id gb_vars = m.addVars(problem.get_n_vars(), vtype=GRB.BINARY) for (lin_id0, lin_id1), cost in problem._pw.items(): # lin_id0 = problem.get_lin_id(prev_frame_id, prev_pose_id) # lin_id1 = problem.get_lin_id(frame_id, pose_id) objective += gb_vars[lin_id0] * gb_vars[lin_id1] * cost for frame_id, lin_ids in problem._constr.items(): constr = None for lin_id in lin_ids: if constr is None: constr = gb_vars[lin_id] else: constr += gb_vars[lin_id] m.addConstr(constr == 1) m.setObjective(objective, GRB.MINIMIZE) # m.solver.callSolver(m) m.optimize() pose_ids = dict() for lin_id, v in enumerate(m.getVars()): print(v.varName, v.x) if v.x > 0.5: frame_id, pose_id = problem.get_frame_id_pose_id(lin_id) assert frame_id not in pose_ids, "no" pose_ids[frame_id] = pose_id # if we have more, pick the first... # if len(pose_in.shape) > 2: # pose_in = pose_in[0, :, :] # pose_in_2d = pose_in_2d[0, :, :] # visible = visible[0] return pose_ids