ax.set_ylabel("x2") a1 = np.array([3,1]) a2 = np.array([2,-2]) a3 = np.array([-1,2]) a4 = np.array([-2,-2]) A = np.array([[3,1], [2,-2], [-1,2], [-2,-2]]) A.shape b = np.ones(4) r = cp.Variable(1) xc = cp.Variable(2) objective = cp.Maximize(r) constraints = [a1 @ xc + r*math.sqrt(a1@a1) <= b[0], a2 @ xc + r*math.sqrt(a2@a2) <= b[1], a3 @ xc + r*math.sqrt(a3@a3) <= b[2], a4 @ xc + r*math.sqrt(a4@a4) <= b[3] ] prob = cp.Problem(objective, constraints) result = prob.solve() print(r.value) print(xc.value)
def dccp_transform(self): """ problem transformation return: prob_new: a new dcp problem parameters: parameters in the constraints flag: indicate if each constraint is transformed parameters_cost: parameters in the cost function flag_cost: indicate if the cost function is transformed var_slack: a list of slack variables """ # split non-affine equality constraints constr = [] for arg in self.constraints: if str(type(arg)) == "<class 'cvxpy.constraints.zero.Zero'>" and not arg.is_dcp(): constr.append(arg[0]<=arg[1]) constr.append(arg[1]<=arg[0]) else: constr.append(arg) self.constraints = constr constr_new = [] # new constraints parameters = [] flag = [] parameters_cost = [] flag_cost = [] # constraints var_slack = [] # slack for constr in self.constraints: if not constr.is_dcp(): flag.append(1) var_slack.append(cvx.Variable(constr.size[0], constr.size[1])) temp = convexify_para_constr(constr) newcon = temp[0] # new constraint without slack variable right = newcon.args[1] + var_slack[-1] # add slack variable on the right side constr_new.append(newcon.args[0]<=right) # new constraint with slack variable constr_new.append(var_slack[-1]>=0) # add constraint on the slack variable parameters.append(temp[1]) for dom in temp[2]: # domain constr_new.append(dom) else: flag.append(0) constr_new.append(constr) # cost functions if not self.objective.is_dcp(): flag_cost.append(1) temp = convexify_para_obj(self.objective) cost_new = temp[0] # new cost function parameters_cost.append(temp[1]) parameters_cost.append(temp[2]) for dom in temp[3]: # domain constraints constr_new.append(dom) else: flag_cost.append(0) cost_new = self.objective.args[0] # objective tau = cvx.Parameter() parameters.append(tau) if self.objective.NAME == 'minimize': for var in var_slack: cost_new += tau*cvx.sum(var) obj_new = cvx.Minimize(cost_new) else: for var in var_slack: cost_new -= tau*cvx.sum(var) obj_new = cvx.Maximize(cost_new) # new problem prob_new = cvx.Problem(obj_new, constr_new) return prob_new, parameters, flag, parameters_cost, flag_cost, var_slack
def __init__(self): self.voltage = cp.Variable() self.current_flows = []
def test_basic_init(pp_fixture, solver_name, i, H, g, x_ineq): """ A basic test case for wrappers. Notice that the input fixture `pp_fixture` is known to have two constraints, one velocity and one acceleration. Hence, in this test, I directly formulate an optimization with cvxpy to test the result. Parameters ---------- pp_fixture: a fixture with only two constraints, one velocity and one acceleration constraint. """ constraints, path, path_discretization, vlim, alim = pp_fixture if solver_name == "cvxpy": solver = cvxpyWrapper(constraints, path, path_discretization) elif solver_name == 'qpOASES': solver = qpOASESSolverWrapper(constraints, path, path_discretization) elif solver_name == 'hotqpOASES': solver = hotqpOASESSolverWrapper(constraints, path, path_discretization) elif solver_name == 'ecos' and H is None: solver = ecosWrapper(constraints, path, path_discretization) else: return True # Skip all other tests xmin, xmax = x_ineq xnext_min = 0 xnext_max = 1 # Results from solverwrapper to test solver.setup_solver() result_ = solver.solve_stagewise_optim(i - 2, H, g, xmin, xmax, xnext_min, xnext_max) result_ = solver.solve_stagewise_optim(i - 1, H, g, xmin, xmax, xnext_min, xnext_max) result = solver.solve_stagewise_optim(i, H, g, xmin, xmax, xnext_min, xnext_max) solver.close_solver() # Results from cvxpy, used as the actual, desired values ux = cvxpy.Variable(2) u = ux[0] x = ux[1] _, _, _, _, _, _, xbound = solver.params[0] a, b, c, F, h, ubound, _ = solver.params[1] Di = path_discretization[i + 1] - path_discretization[i] v = a[i] * u + b[i] * x + c[i] cvxpy_constraints = [ u <= ubound[i, 1], u >= ubound[i, 0], x <= xbound[i, 1], x >= xbound[i, 0], F[i] * v <= h[i], x + u * 2 * Di <= xnext_max, x + u * 2 * Di >= xnext_min, ] if xmin is not None: cvxpy_constraints.append(x <= xmax) cvxpy_constraints.append(x >= xmin) if H is not None: objective = cvxpy.Minimize(0.5 * cvxpy.quad_form(ux, H) + g * ux) else: objective = cvxpy.Minimize(g * ux) problem = cvxpy.Problem(objective, cvxpy_constraints) if FOUND_MOSEK: problem.solve(solver="MOSEK", verbose=True) else: problem.solve(solver="ECOS", verbose=True) if problem.status == "optimal": actual = np.array(ux.value).flatten() npt.assert_allclose(result.flatten(), actual.flatten(), atol=5e-3, rtol=1e-5) # Very bad accuracy? why? else: assert np.all(np.isnan(result))
def OptimizeTraj(self, waypoints): # Initial precomputed path (waypoint based) wp = np.matrix(waypoints) # Number of states: position and velocity on x and y directions n = 4 # Number of inputs: thrust on x and y directions m = 2 # Number of iterations K = len(waypoints[0]) print(K) # Total time T = self.h * K print(T) Ldt = [] for l in range(K + 1): Ldt.append(l * self.h) print(Ldt) # Kinematics of a point mass A = np.matrix([[1, 0, self.h, 0], [0, 1, 0, self.h], [0, 0, 1, 0], [0, 0, 0, 1]]) B = np.matrix([[(self.h**2) / 2, 0], [0, (self.h**2) / 2], [self.h, 0], [0, self.h]]) # Start state x_0 = np.array([self.start.x, self.start.y, 0, 0]) # Form and solve control problem. x = cp.Variable((n, K + 1)) u = cp.Variable((m, K)) cost = 0 constr = [] for t in range(0, K): cost += cp.sum_squares(u[:, t]) constr += [ x[:, t + 1] == A @ x[:, t] + B @ u[:, t], cp.norm(wp[:, t] - x[:2, t][:, None], 'inf') <= self.minDist, cp.norm(u[:, t], 'inf') <= self.accMax, cp.norm(x[2:, t], 'inf') <= self.velMax ] # sums problem objectives and concatenates constraints with the initial and final states. constr += [ x[:, K] == np.array([self.goal.x, self.goal.y, 1, 1]), x[:, 0] == x_0 ] # Time taken until this point #end = time.time() #print('Problem formulation:',end - start) problem = cp.Problem(cp.Minimize(cost), constr) problem.solve(verbose=False) if problem.status not in ["infeasible", "unbounded"]: # Otherwise, problem.value is inf or -inf, respectively. print("Optimal value: %s" % problem.value) for variable in problem.variables(): print("Variable %s: value %s" % (variable.name(), variable.value)) plt.figure(2) # Plot (x_t)_3. plt.subplot(2, 2, 1) x3 = (x[2, :].value).tolist() print(x3) plt.plot(Ldt, x3) #plt.axis([-self.velMax, T, -self.velMax, self.velMax]) plt.ylabel(r"$v_x[m/s]$", fontsize=16) plt.ylim([-self.velMax, self.velMax]) #plt.xticks([]) plt.xlabel(r"$t[s]$", fontsize=16) plt.xlim([0, T]) plt.grid(True) # Plot (x_t)_4. plt.subplot(2, 2, 2) x4 = x[3, :].value #print(x4) plt.plot(range(K + 1), x4) plt.ylabel(r"$v_y[m/s]$", fontsize=16) plt.ylim([-self.velMax, self.velMax]) #plt.xticks([]) plt.xlabel(r"$t[s]$", fontsize=16) plt.xlim([0, T]) plt.grid(True) # Plot (u_t)_1. plt.subplot(2, 2, 3) plt.plot(u[0, :].value) plt.ylabel(r"$a_x[m/(s*s)]$", fontsize=16) plt.ylim([-self.accMax, self.accMax]) #plt.yticks(np.linspace(-1, 1, 3)) #plt.xticks([]) plt.xlabel(r"$t[s]$", fontsize=16) plt.xlim([0, T]) plt.tight_layout() plt.grid(True) # Plot (u_t)_2. plt.subplot(2, 2, 4) plt.plot(u[1, :].value) plt.ylabel(r"$a_y[m/(s*s)]$", fontsize=16) plt.ylim([-self.accMax, self.accMax]) #plt.yticks(np.linspace(-1, 1, 3)) #plt.xticks([]) plt.xlabel(r"$t[s]$", fontsize=16) plt.xlim([0, T]) plt.tight_layout() plt.grid(True) stateMat = np.matrix( [x[0, :].value, x[1, :].value, x[2, :].value, x[3, :].value]) ctrlMat = np.matrix([u[0, :].value, u[1, :].value]) #print(ctrlMat) #print(x) # Returnam matricea cu starile pentru a extrage din aceasta pozitia (x,y), in vederea reprezentarii grafice return stateMat
def eval(self, writer): td3_results, mql_results = [], [] for i, (test_task_idx, test_buffer) in enumerate(zip(self.task_config.test_tasks, self._test_buffers)): batch = test_buffer.sample(self._args.batch_size, return_dict=True) other_batches = [buf.sample(self._args.batch_size, return_dict=True) for buf in self._inner_buffers] self._env.set_task_idx(test_task_idx) state, action, next_state, reward = (torch.tensor(batch['obs']).to(self._args.device), torch.tensor(batch['actions']).to(self._args.device), torch.tensor(batch['next_obs']).to(self._args.device), torch.tensor(batch['rewards']).to(self._args.device)) with torch.no_grad(): context_input = torch.cat((state, action, next_state, reward), -1) context_seq, h_n = self.context_encoder(context_input.unsqueeze(1), self.c0) context = h_n[-1] all_test_context = context_seq.squeeze(1) all_other_context, (state_, action_, next_state_, reward_) = self.get_context_from_batches(other_batches) all_other_context = all_other_context[torch.randperm(all_other_context.shape[0], device=self._args.device)[:self._args.batch_size]] labels = np.concatenate((-np.ones((self._args.batch_size)), np.ones((self._args.batch_size)))) X = torch.cat((all_test_context, all_other_context)).cpu().numpy() w = cp.Variable((all_test_context.shape[-1])) obj = cp.Minimize(cp.sum(cp.logistic(cp.neg(cp.multiply(labels, X @ w))))) prob = cp.Problem(obj) sol = prob.solve() w_ = torch.tensor(w.value, device=self._args.device).float() test_betas = (-all_test_context @ w_).exp() test_ess = (1/test_betas.shape[0]) * test_betas.sum().pow(2) / test_betas.pow(2).sum() context_betas = (-all_other_context @ w_).exp() traj, reward, success = self._rollout_policy(self._env, context) td3_results.append({'reward': reward, 'success': success, 'task': test_task_idx}) writer.add_scalar(f'TD3_Reward/Task_{test_task_idx}', reward, self.total_it) writer.add_scalar(f'TD3_Success/Task_{test_task_idx}', success, self.total_it) actor = copy.deepcopy(self.actor) critic_target = copy.deepcopy(self.critic_target) critic = copy.deepcopy(self.critic) actor_optimizer = torch.optim.Adam(actor.parameters(), lr=3e-4) critic_optimizer = torch.optim.Adam(critic.parameters(), lr=3e-4) start_actor_params = [p.clone() for p in actor.parameters()] start_critic_params = [p.clone() for p in critic.parameters()] lambda_ = 1 - test_ess context_ = context for step in range(self._mql_steps1 + self._mql_steps2): if step == self._mql_steps1: traj, reward, success = self._rollout_policy(self._env, context, policy=actor) writer.add_scalar(f'MQL1_Reward/Task_{test_task_idx}', reward, self.total_it) writer.add_scalar(f'MQL1_Success/Task_{test_task_idx}', success, self.total_it) critic_param_loss = sum([(p - p_.detach()).pow(2).sum() for p, p_ in zip(critic.parameters(), start_critic_params)]) actor_param_loss = sum([(p - p_.detach()).pow(2).sum() for p, p_ in zip(actor.parameters(), start_actor_params)]) if step >= self._mql_steps1: # re-assign state, action, next_state, reward to use data from train buffers # use w_ to assign weights # also add regularization to theta other_batches = [buf.sample(self._args.batch_size, return_dict=True) for buf in self._inner_buffers] all_other_context, (state, action, next_state, reward) = self.get_context_from_batches(other_batches) context_betas = (-all_other_context @ w_).exp() context_ess = (1/context_betas.shape[0]) * context_betas.sum().pow(2) / context_betas.pow(2).sum() lambda_ = 1 - context_ess if step == self._mql_steps1 + self._mql_steps2 - 1: writer.add_scalar(f'Test_Beta/Task_{test_task_idx}', test_betas.mean(), self.total_it) writer.add_scalar(f'Context_Beta/Task_{test_task_idx}', context_betas.mean(), self.total_it) writer.add_scalar(f'Test_ESS/Task_{test_task_idx}', test_ess, self.total_it) writer.add_scalar(f'Context_ESS/Task_{test_task_idx}', context_ess, self.total_it) else: lambda_ = 1 - test_ess not_done = torch.ones((state.shape[0],1)).to(self._args.device) if context_.shape[0] != state.shape[0]: context_ = context.repeat(state.shape[0], 1) with torch.no_grad(): next_action = actor(next_state, context_) # Compute the target Q value target_Q1, target_Q2 = critic_target(next_state, next_action, context_) target_Q = torch.min(target_Q1, target_Q2) target_Q = reward + not_done * self.discount * target_Q # Get current Q estimates current_Q1, current_Q2 = critic(state, action, context_) # Compute critic loss critic_loss = F.mse_loss(current_Q1, target_Q) + F.mse_loss( current_Q2, target_Q) + (lambda_ / 2) * critic_param_loss # Optimize the critic critic_optimizer.zero_grad() critic_loss.backward(retain_graph=True) critic_optimizer.step() # Compute actor losse actor_loss = -critic.Q1(state, actor(state, context_), context_).mean() + (lambda_ / 2) * actor_param_loss # Optimize the actor actor_optimizer.zero_grad() actor_loss.backward() actor_optimizer.step() for param, target_param in zip(critic.parameters(), critic_target.parameters()): target_param.data.copy_(self.tau * param.data + (1 - self.tau) * target_param.data) traj, reward, success = self._rollout_policy(self._env, context, policy=actor) mql_results.append({'reward': reward, 'success': success, 'task': test_task_idx}) writer.add_scalar(f'MQL_Reward/Task_{test_task_idx}', reward, self.total_it) writer.add_scalar(f'MQL_Success/Task_{test_task_idx}', success, self.total_it) return td3_results, mql_results
def worst_case_LMM(L, mu, gamma, alpha, beta, G, F, x, N=2, eps=0.): """ Evaluate one of the worst-case function at a new point x Parameters ---------- L : float smoothness parameter for the class of functions mu : float strong-convexity parameter for the class of functions gamma : float step size of the method alpha : ndarray, shape (s,) internal step size beta : ndarray, shape (s,) external step size G : np.ndarray , shape ((2+s)*2, (2+s)*2), Gram matrix F : np.ndarray, shape ((2+s)*2, ) function values x : np.ndarray, shape (N,) values in which the function is evaluated N : int dimension to keep eps : float error to avoid violating the interpolation constraints after QR decomposition of G Returns ------- f(x) worst-case function evaluated in x """ ## QR DECOMPOSITION ON G, WITH N MAIN DIMENSIONS # Find R such that R^TR=G eigenvalues, eigenvector = np.linalg.eig(G) eigs = np.zeros(len(eigenvalues)) for i in range(N): eigs[i] = eigenvalues[i] new_D = np.diag(np.sqrt(eigs)) Q, R = np.linalg.qr(np.dot(new_D, eigenvector.T)) # Keep the principal dimensions R = R[:N, :] # Reconstruction of x_i based on the approximating R XX = [R[:, 0], R[:, 1], R[:, 2], R[:, 3]] # initial points YY = [] GG = [] for i in range(4, R.shape[1]): GG.append(R[:, i]) # basis # Iterates for i in range(4, R.shape[1], 2): YY.append(XX[i - 4] * beta[0] + beta[1] * XX[i - 2]) # y_1i YY.append(XX[i - 3] * beta[0] + beta[1] * XX[i - 1]) # y_2i XX.append(-alpha[1] * XX[i - 2] - alpha[0] * XX[i - 4] - gamma * GG[i - 4]) # x_1i XX.append(-alpha[1] * XX[i - 1] - alpha[0] * XX[i - 3] - gamma * GG[i - 3]) # x_2i ## INTERPOLATION OF THE FUNCTION (QCQP PROBLEM) m1 = np.array([[-mu * L, mu * L], [mu * L, -mu * L]]) m2 = np.array([[mu, -L], [-mu, L]]) m3 = np.array([[-1, 1], [1, -1]]) M1 = np.kron(m1, np.eye(R.shape[0])) M2 = np.kron(m2, np.eye(R.shape[0])) M3 = np.kron(m3, np.eye(R.shape[0])) f = cp.Variable(1) g = cp.Variable(R.shape[0]) constraints = [f >= 0.] for i in range(R.shape[1] - 4): X = np.array([x[0], x[1], YY[i][0], YY[i][1]]).T G = np.array([0, 0, GG[i][0], GG[i][1]]).T U1 = np.zeros((2, 4)) U1[0][0] = 1. U1[1][1] = 1. G = G + g @ U1 X_ = np.array([YY[i][0], YY[i][1], x[0], x[1]]).T G_ = np.array([GG[i][0], GG[i][1], 0, 0]).T U2 = np.zeros((2, 4)) U2[0][2] = 1. U2[1][3] = 1. G_ = G_ + g @ U2 constraints = constraints + [ (L - mu) * (f - F[i][0]) + 1 / 2 * (X.T @ M1 @ X + 2 * (X.T @ M2 @ G) + cp.quad_form(G, M3)) >= -eps ] constraints = constraints + [ (L - mu) * (F[i][0] - f) + 1 / 2 * (X_.T @ M1 @ X_ + 2 * (X_.T @ M2 @ G_) + cp.quad_form(G_, M3)) >= -eps ] prob = cp.Problem(cp.Minimize(f), constraints) prob.solve(cp.SCS) print( 'The interpolating function evaluated at point {}is equal to'.format( str(x)), prob.value) return prob.value, YY, XX, GG, g.value
if myneighbor not in convexNodesArr: convexNodes[val[1]] = convexNodesCount convexNodesArr.append(myneighbor) derivedFrom[myneighbor] = parent convexNodesCount += 1 #print(len(observationCascades.keys())) #time.sleep(1) num_nodes = len(convexNodesArr) # prepare the observation matrix #A = np.zeros((num_nodes, num_nodes), dtype=float) # This is one column of the alpha matrix Ai = CVX.Variable(num_nodes, name='A[:,{}]'.format(convexNodes[target_node])) constraints = [] # define constraints for j in range(num_nodes): if j == convexNodes[target_node]: constraints.append(Ai[j] == 0) else: constraints.append(Ai[j] >= 0) #constraints.append(Ai[j] >> 0) # this line is used if we are interested in the neighborhood # cascade as well, but to do this we need to load all the cascades """ # nothing in cascades related to this node, just skip this value if target_node not in nodeCascades.keys():
import cvxpy as cp import numpy as np v = np.array([1,1,1,1]) # edge weight from {a,b,c,d} to {e,f,g,h} c = np.array([[1,0,3,0], [0,2,0,4], [0,0,0,5], [0,1,2,0] ]) # boolean matrix to check whether the edge will result in maximum bipartite graph or not x = cp.Variable((4,4), boolean=True) y=[0,0,0,0] constraints =[] for i in range(0,4): sum1=0 sum2=0 for j in range(0,4): sum1+=x[i][j] sum2+=x[j][i] #the number of edges between any two vertices should be <=1. Therefore these constraints ensure that no row or column should sum upto more than one. constraints+=[sum1<=v[i]] constraints+=[sum2<=v[i]] #maximizing the set of edges prob = cp.Problem(cp.Maximize(cp.sum(cp.multiply(c,x))), constraints) prob.solve() print("The maximum weight is", prob.value) print("The edge matrix is",x.value)
def test_absorb_lin_op(self): """Test absorb lin op operator. """ # norm1. tmp = Variable(10) v = np.arange(10) * 1.0 - 5.0 fn = norm1(mul_elemwise(-v, tmp), alpha=5.) rho = 2 new_prox = absorb_lin_op(fn)[0] x = new_prox.prox(rho, v.copy()) self.assertItemsAlmostEqual( x, np.sign(v) * np.maximum(np.abs(v) - 5. * np.abs(v) / rho, 0)) fn = norm1(mul_elemwise(-v, mul_elemwise(2 * v, tmp)), alpha=5.) rho = 2 new_prox = absorb_lin_op(fn)[0] x = new_prox.prox(rho, v.copy()) self.assertItemsAlmostEqual( x, np.sign(v) * np.maximum(np.abs(v) - 5. * np.abs(v) / rho, 0)) new_prox = absorb_lin_op(new_prox)[0] x = new_prox.prox(rho, v.copy()) new_v = 2 * v * v self.assertItemsAlmostEqual( x, np.sign(new_v) * np.maximum(np.abs(new_v) - 5. * np.abs(new_v) / rho, 0)) # nonneg. tmp = Variable(10) v = np.arange(10) * 1.0 - 5.0 fn = nonneg(mul_elemwise(-v, tmp), alpha=5.) rho = 2 new_prox = absorb_lin_op(fn)[0] x = new_prox.prox(rho, v.copy()) self.assertItemsAlmostEqual(x, fn.prox(rho, -np.abs(v))) # sum_squares. tmp = Variable(10) v = np.arange(10) * 1.0 - 5.0 alpha = 5. val = np.arange(10) fn = sum_squares(mul_elemwise(-v, tmp), alpha=alpha, c=val) rho = 2 new_prox = absorb_lin_op(fn)[0] x = new_prox.prox(rho, v.copy()) cvx_x = cvx.Variable(10) prob = cvx.Problem( cvx.Minimize( cvx.sum_squares(cvx_x - v) * (rho / 2) + 5 * cvx.sum_squares(cvx.mul_elemwise(-v, cvx_x)) + (val * -v).T * cvx_x)) prob.solve() self.assertItemsAlmostEqual(x, cvx_x.value, places=3) # Test scale. tmp = Variable(10) v = np.arange(10) * 1.0 - 5.0 fn = norm1(10 * tmp) rho = 2 new_prox = absorb_lin_op(fn)[0] x = new_prox.prox(rho, v.copy()) cvx_x = cvx.Variable(10) prob = cvx.Problem( cvx.Minimize(cvx.sum_squares(cvx_x - v) + cvx.norm(10 * cvx_x, 1))) prob.solve() self.assertItemsAlmostEqual(x, cvx_x.value, places=3) val = np.arange(10) fn = norm1(10 * tmp, c=val, b=val, gamma=0.01) rho = 2 new_prox = absorb_lin_op(fn)[0] x = new_prox.prox(rho, v.copy()) cvx_x = cvx.Variable(10) prob = cvx.Problem(cvx.Minimize(cvx.sum_squares(cvx_x - v) + cvx.norm(10 * cvx_x - val, 1) + 10 * val.T * \ cvx_x + cvx.sum_squares(cvx_x) )) prob.solve() self.assertItemsAlmostEqual(x, cvx_x.value, places=2) # sum_entries tmp = Variable(10) v = np.arange(10) * 1.0 - 5.0 fn = sum_entries(sum([10 * tmp, mul_elemwise(v, tmp)])) funcs = absorb.absorb_all_lin_ops([fn]) c = __builtins__['sum']([func.c for func in funcs]) self.assertItemsAlmostEqual(c, v + 10, places=3)
def runOptimiser(K, u, preOptw, initialValue, optimizer, maxWeight=10000): """ Args: K (double 2d array): Similarity/distance matrix u (double array): Mean similarity of each prototype preOptw (double): Weight vector initialValue (double): Initialize run optimizer (string): qpsolver ('cvxpy' or 'osqp') maxWeight (double): Upper bound on weight Returns: Prototypes, weights and objective values """ # Standard QP: # minimize # (1/2) * x.T * P * x + q.T * x # subject to # G * x <= h # A * x == b # QP Solved by Protodash: # minimize # (1/2) * x.T * K * x + (-u).T * x # subject to # G * x <= h assert (optimizer=='cvxpy' or optimizer=='osqp'), "Please set optimizer as 'cvxpy' or 'osqp'" d = u.shape[0] lb = np.zeros((d, 1)) ub = maxWeight * np.ones((d, 1)) # x0 = initial value, provided optimizer supports it. x0 = np.append( preOptw, initialValue/K[d-1, d-1] ) G = np.vstack((np.identity(d), -1*np.identity(d))) h = np.vstack((ub, -1*lb)).ravel() # variable shapes: K = (d,d), u = (d,) G = (2d, d), h = (2d,) if (optimizer == 'cvxpy'): x = cp.Variable(d) prob = cp.Problem(cp.Minimize((1/2)*cp.quad_form(x, K) + (-u).T@x), [G@x <= h]) prob.solve() xv = x.value.reshape(-1, 1) xreturn = x.value elif (optimizer == 'osqp'): Ks = sparse.csc_matrix(K) Gs = sparse.csc_matrix(G) l_inf = -np.inf * np.ones(len(h)) solver = OSQP() solver.setup(P=Ks, q=-u, A=Gs, l=l_inf, u=h, eps_abs=1e-4, eps_rel=1e-4, polish= True, verbose=False) solver.warm_start(x=x0) res = solver.solve() xv = res.x.reshape(-1, 1) xreturn = res.x # compute objective function value P = K q = - u.reshape(-1, 1) obj_value = 1/2 * np.matmul(np.matmul(xv.T, P), xv) + np.matmul(q.T, xv) return(xreturn, obj_value[0,0])
def get_params(data): r"""Correct a signal estimated as numerator/denominator for weekday effects. The ordinary estimate would be numerator_t/denominator_t for each time point t. Instead, model log(numerator_t/denominator_t) = alpha_{wd(t)} + phi_t where alpha is a vector of fixed effects for each weekday. For identifiability, we constrain \sum alpha_j = 0, and to enforce this we set Sunday's fixed effect to be the negative sum of the other weekdays. We estimate this as a penalized Poisson GLM problem with log link. We rewrite the problem as log(numerator_t) = alpha_{wd(t)} + phi_t + log(denominator_t) and set a design matrix X with one row per time point. The first six columns of X are weekday indicators; the remaining columns are the identity matrix, so that each time point gets a unique phi. Using this X, we write log(numerator_t) = X beta + log(denominator_t) Hence the first six entries of beta correspond to alpha, and the remaining entries to phi. The penalty is on the L1 norm of third differences of phi (so the third differences of the corresponding columns of beta), to enforce smoothness. Third differences ensure smoothness without removing peaks or valleys. Objective function is negative mean Poisson log likelihood plus penalty: ll = (numerator * (X*b + log(denominator)) - sum(exp(X*b) + log(denominator))) / num_days Return a matrix of parameters: the entire vector of betas, for each time series column in the data. """ tmp = data.reset_index() denoms = tmp.groupby(Config.DATE_COL).sum()["den"] nums = tmp.groupby(Config.DATE_COL).sum()["num"] n_nums = 1 # only one numerator column # Construct design matrix to have weekday indicator columns and then day # indicators. X = np.zeros((nums.shape[0], 6 + nums.shape[0])) # pylint: disable=invalid-name not_sunday = np.where(nums.index.dayofweek != 6)[0] X[not_sunday, np.array(nums.index.dayofweek)[not_sunday]] = 1 X[np.where(nums.index.dayofweek == 6)[0], :6] = -1 X[:, 6:] = np.eye(X.shape[0]) npnums, npdenoms = np.array(nums), np.array(denoms) params = np.zeros((n_nums, X.shape[1])) # fit model b = cp.Variable((X.shape[1])) lmbda = cp.Parameter(nonneg=True) lmbda.value = 10 # Hard-coded for now, seems robust to changes ll = (cp.matmul(npnums, cp.matmul(X, b) + np.log(npdenoms)) - cp.sum(cp.exp(cp.matmul(X, b) + np.log(npdenoms)))) / X.shape[0] penalty = (lmbda * cp.norm(cp.diff(b[6:], 3), 1) / (X.shape[0] - 2) ) # L-1 Norm of third differences, rewards smoothness try: prob = cp.Problem(cp.Minimize(-ll + lmbda * penalty)) _ = prob.solve() except SolverError: # If the magnitude of the objective function is too large, an error is # thrown; Rescale the objective function prob = cp.Problem(cp.Minimize((-ll + lmbda * penalty) / 1e5)) _ = prob.solve() params = b.value return params
def _calc_stock_pos(self, trade_date): stock_ids = self._load_stock_ids(trade_date) trade_dates_total = self.trade_dates_total.copy() factor_exposure_date = trade_dates_total[ trade_dates_total < trade_date].sort_values()[-1] df_FactorExposure_t = self._df_stock_factor_exposure.loc[ factor_exposure_date].copy() df_FactorExposure_t = df_FactorExposure_t.loc[ df_FactorExposure_t.index.isin(stock_ids)] df_FactorExposure_t[self.style_factor] = df_FactorExposure_t[ self.style_factor].fillna( df_FactorExposure_t[self.style_factor].mean()) df_FactorExposure_t['weight'] = df_FactorExposure_t[ 'weight'] / df_FactorExposure_t['weight'].sum() ser_IndexFactorExposure = pd.Series(data=np.dot( df_FactorExposure_t[self.style_factor].T, df_FactorExposure_t['weight']), index=[self.style_factor]) ser_EstStockReturn = pd.Series(data=0.0, index=df_FactorExposure_t.index) for j_target_exposure in self.target_exposure: if j_target_exposure == self.target_exposure.min(): max_exposure = self.target_exposure.min( ) + self.exposure_target_interval / 2 min_exposure = -np.inf elif j_target_exposure == self.target_exposure.max(): max_exposure = np.inf min_exposure = self.target_exposure.max( ) - self.exposure_target_interval / 2 else: max_exposure = j_target_exposure + self.exposure_target_interval / 2 min_exposure = j_target_exposure - self.exposure_target_interval / 2 df_FactorExposure_t2 = df_FactorExposure_t[ self.style_factor].applymap(lambda x: self.calc_stock_return( x, upper_limit=max_exposure, lower_limit=min_exposure)) df_FactorReturn_t = self._dict_FactorReturn[ j_target_exposure].loc[:factor_exposure_date].iloc[ -self.observe_period:].copy() # t_value notice: 自相关导致该t值会显著高估,后期应进行修改 ser_t_values_t = df_FactorReturn_t.mean() * np.sqrt( df_FactorReturn_t.shape[0]) / df_FactorReturn_t.std() ser_FactorReturn_t = df_FactorReturn_t.mean() * 242 # adjust factor return ser_AdjFactorReturn_t = pd.Series(data=0.0, index=self.style_factor) for k_style_factor in self.style_factor: if j_target_exposure >= 0.0: if ser_t_values_t[ k_style_factor] > self.tvalues_threshold_p and k_style_factor in self.alpha_factor_p: ser_AdjFactorReturn_t[ k_style_factor] = ser_FactorReturn_t[ k_style_factor] elif ser_t_values_t[ k_style_factor] < -self.tvalues_threshold_n and k_style_factor in self.alpha_factor_n: ser_AdjFactorReturn_t[ k_style_factor] = ser_FactorReturn_t[ k_style_factor] else: pass else: # j_target_exposure < 0.0: if ser_t_values_t[ k_style_factor] < -self.tvalues_threshold_n and k_style_factor in self.alpha_factor_p: ser_AdjFactorReturn_t[ k_style_factor] = ser_FactorReturn_t[ k_style_factor] elif ser_t_values_t[ k_style_factor] > self.tvalues_threshold_p and k_style_factor in self.alpha_factor_n: ser_AdjFactorReturn_t[ k_style_factor] = ser_FactorReturn_t[ k_style_factor] else: pass # ser_EstStockReturn_t = pd.Series(data=np.dot( df_FactorExposure_t2.values, ser_AdjFactorReturn_t.values), index=df_FactorExposure_t2.index) ser_EstStockReturn = ser_EstStockReturn + ser_EstStockReturn_t # considering fee if trade_date == self.reindex[0]: ser_AddStockReturn = self._df_stock_pos.loc[trade_date].reindex( ser_EstStockReturn.index).fillna(0.0) else: last_trade_date = self.reindex[self.reindex < trade_date][-1] ser_AddStockReturn = self._df_stock_pos.loc[ last_trade_date].reindex(ser_EstStockReturn.index).fillna(0.0) ser_AddStockReturn[ser_AddStockReturn >= self.max_percentage / 10] = self.trading_fee ser_EstStockReturn = ser_EstStockReturn + ser_AddStockReturn n = df_FactorExposure_t.shape[0] x = cp.Variable(n) G_RiskFactor = df_FactorExposure_t[self.risk_factor].values.T h_RiskFactor = ser_IndexFactorExposure.loc[self.risk_factor].values obj = cp.Minimize(-x @ ser_EstStockReturn.values) G_AlphaFactorP = df_FactorExposure_t[self.alpha_factor_p].values.T h_AlphaFactorP = np.ones(len( self.alpha_factor_p)) * self.max_factor_exposure G_AlphaFactorN = df_FactorExposure_t[self.alpha_factor_n].values.T h_AlphaFactorN = -np.ones(len( self.alpha_factor_n)) * self.max_factor_exposure con = [ x >= 0, x <= self.max_percentage, x @ np.ones(n) == 1.0, G_RiskFactor @ x == h_RiskFactor, G_AlphaFactorP @ x <= h_AlphaFactorP, G_AlphaFactorP @ x >= 0.0, G_AlphaFactorN @ x >= h_AlphaFactorN, G_AlphaFactorN @ x <= 0.0, ] prob = cp.Problem(obj, con) if prob.solve() in [np.inf, -np.inf]: print(trade_date, 'error') else: print(trade_date, 'success') df_FactorExposure_t['opt_weight'] = x.value df_FactorExposure_t.loc[ df_FactorExposure_t.opt_weight < self.max_percentage / 10, 'opt_weight'] = 0.0 df_FactorExposure_t['opt_weight'] = df_FactorExposure_t[ 'opt_weight'] / df_FactorExposure_t['opt_weight'].sum() stock_pos = df_FactorExposure_t.opt_weight stock_pos.name = trade_date return stock_pos
n = 2 m1 = A.shape[0] m2 = B.shape[0] H = np.hstack([A, np.ones((m1, 1))]) G = np.hstack([B, np.ones((m2, 1))]) D1_SR = cp.Parameter((m1, m1)) D2_SR = cp.Parameter((m2, m2)) c1 = cp.Parameter(nonneg=True) c2 = cp.Parameter(nonneg=True) # Regularizor Coeff c1.value = 0.001 c2.value = 0.001 # Construct the problem. z1 = cp.Variable(n + 1) q1 = cp.Variable(m2) z2 = cp.Variable(n + 1) q2 = cp.Variable(m1) init_obj1 = cp.Minimize(cp.sum_squares(H * z1) / 2 + c1 * cp.sum(q1)) init_cons1 = [-G * z1 + q1 >= 1, q1 >= 0] init_cons1.append(z1[0] == 1) init_prob1 = cp.Problem(init_obj1, init_cons1) init_obj2 = cp.Minimize(cp.sum_squares(G * z2) / 2 + c2 * cp.sum(q2)) init_cons2 = [H * z2 + q2 >= 1, q2 >= 0] init_cons2.append(z2[0] == 1) init_prob2 = cp.Problem(init_obj2, init_cons2) objective1 = cp.Minimize(cp.sum_squares(D1_SR * H * z1) / 2 + c1 * cp.sum(q1)) constraints1 = [-G * z1 + q1 >= 1, q1 >= 0]
def test_intro(self): """Test examples from cvxpy.org introduction. """ import numpy # Problem data. m = 30 n = 20 numpy.random.seed(1) A = numpy.random.randn(m, n) b = numpy.random.randn(m) # Construct the problem. x = Variable(n) objective = Minimize(sum_squares(A*x - b)) constraints = [0 <= x, x <= 1] prob = Problem(objective, constraints) # The optimal objective is returned by p.solve(). result = prob.solve() # The optimal value for x is stored in x.value. print x.value # The optimal Lagrange multiplier for a constraint # is stored in constraint.dual_value. print constraints[0].dual_value ######################################## # Create two scalar variables. x = Variable() y = Variable() # Create two constraints. constraints = [x + y == 1, x - y >= 1] # Form objective. obj = Minimize(square(x - y)) # Form and solve problem. prob = Problem(obj, constraints) prob.solve() # Returns the optimal value. print "status:", prob.status print "optimal value", prob.value print "optimal var", x.value, y.value ######################################## import cvxpy as cvx # Create two scalar variables. x = cvx.Variable() y = cvx.Variable() # Create two constraints. constraints = [x + y == 1, x - y >= 1] # Form objective. obj = cvx.Minimize(cvx.square(x - y)) # Form and solve problem. prob = cvx.Problem(obj, constraints) prob.solve() # Returns the optimal value. print "status:", prob.status print "optimal value", prob.value print "optimal var", x.value, y.value self.assertEqual(prob.status, OPTIMAL) self.assertAlmostEqual(prob.value, 1.0) self.assertAlmostEqual(x.value, 1.0) self.assertAlmostEqual(y.value, 0) ######################################## # Replace the objective. prob.objective = Maximize(x + y) print "optimal value", prob.solve() self.assertAlmostEqual(prob.value, 1.0) # Replace the constraint (x + y == 1). prob.constraints[0] = (x + y <= 3) print "optimal value", prob.solve() self.assertAlmostEqual(prob.value, 3.0) ######################################## x = Variable() # An infeasible problem. prob = Problem(Minimize(x), [x >= 1, x <= 0]) prob.solve() print "status:", prob.status print "optimal value", prob.value self.assertEquals(prob.status, INFEASIBLE) self.assertAlmostEqual(prob.value, np.inf) # An unbounded problem. prob = Problem(Minimize(x)) prob.solve() print "status:", prob.status print "optimal value", prob.value self.assertEquals(prob.status, UNBOUNDED) self.assertAlmostEqual(prob.value, -np.inf) ######################################## # A scalar variable. a = Variable() # Column vector variable of length 5. x = Variable(5) # Matrix variable with 4 rows and 7 columns. A = Variable(4, 7) ######################################## import numpy # Problem data. m = 10 n = 5 numpy.random.seed(1) A = numpy.random.randn(m, n) b = numpy.random.randn(m) # Construct the problem. x = Variable(n) objective = Minimize(sum_entries(square(A*x - b))) constraints = [0 <= x, x <= 1] prob = Problem(objective, constraints) print "Optimal value", prob.solve() print "Optimal var" print x.value # A numpy matrix. self.assertAlmostEqual(prob.value, 4.14133859146) ######################################## # Positive scalar parameter. m = Parameter(sign="positive") # Column vector parameter with unknown sign (by default). c = Parameter(5) # Matrix parameter with negative entries. G = Parameter(4, 7, sign="negative") # Assigns a constant value to G. G.value = -numpy.ones((4, 7)) ######################################## import numpy # Problem data. n = 15 m = 10 numpy.random.seed(1) A = numpy.random.randn(n, m) b = numpy.random.randn(n) # gamma must be positive due to DCP rules. gamma = Parameter(sign="positive") # Construct the problem. x = Variable(m) sum_of_squares = sum_entries(square(A*x - b)) obj = Minimize(sum_of_squares + gamma*norm(x, 1)) prob = Problem(obj) # Construct a trade-off curve of ||Ax-b||^2 vs. ||x||_1 sq_penalty = [] l1_penalty = [] x_values = [] gamma_vals = numpy.logspace(-4, 6) for val in gamma_vals: gamma.value = val prob.solve() # Use expr.value to get the numerical value of # an expression in the problem. sq_penalty.append(sum_of_squares.value) l1_penalty.append(norm(x, 1).value) x_values.append(x.value) ######################################## import numpy X = Variable(5, 4) A = numpy.ones((3, 5)) # Use expr.size to get the dimensions. print "dimensions of X:", X.size print "dimensions of sum_entries(X):", sum_entries(X).size print "dimensions of A*X:", (A*X).size # ValueError raised for invalid dimensions. try: A + X except ValueError, e: print e
m = 50 n = 100 lam = float(0.01) import scipy.sparse as sps A = sps.rand(n, n, 0.01) A = np.asarray(A.T.dot(A).todense() + 0.1 * np.eye(n)) L = np.linalg.cholesky(np.linalg.inv(A)) X = np.random.randn(m, n).dot(L.T) S = X.T.dot(X) / m W = np.ones((n, n)) - np.eye(n) # Problem construction Theta = cp.Variable(n, n) prob = cp.Problem( cp.Minimize(lam * cp.norm1(cp.mul_elemwise(W, Theta)) + cp.sum_entries(cp.mul_elemwise(S, Theta)) - cp.log_det(Theta))) # Problem collection # Single problem collection problemDict = {"problemID": problemID, "problem": prob, "opt_val": opt_val} problems = [problemDict] # For debugging individual problems: if __name__ == "__main__": def printResults(problemID="", problem=None, opt_val=None): print(problemID)
def model_spec_solve(A, b, A_const, b_const, fm, convhull=True, eps=0.001, verbose=False, plot=False, fairtype='EOd'): fct_model = np.array([ fm.pos_group_stats['TP'], fm.pos_group_stats['FN'], fm.pos_group_stats['FP'], fm.pos_group_stats['TN'], fm.neg_group_stats['TP'], fm.neg_group_stats['FN'], fm.neg_group_stats['FP'], fm.neg_group_stats['TN'] ]) fct_model = fct_model / np.sum(fct_model) N = fm.y_test.shape[0] M_pos = fm.mu_pos / N NM_pos = fm.pos_group_num / N - M_pos M_neg = fm.mu_neg / N NM_neg = fm.neg_group_num / N - M_neg z = cp.Variable(8) c = np.array([[0, 1, 1, 0, 0, 1, 1, 0]]) fpr_pos_1 = max(fct_model[2] / NM_pos, fct_model[3] / NM_pos) fpr_pos_2 = 1 - fpr_pos_1 tpr_pos_1 = min(fct_model[0] / M_pos, fct_model[1] / M_pos) tpr_pos_2 = 1 - tpr_pos_1 fpr_neg_1 = max(fct_model[6] / NM_neg, fct_model[7] / NM_neg) fpr_neg_2 = 1 - fpr_neg_1 tpr_neg_1 = min(fct_model[4] / M_neg, fct_model[5] / M_neg) tpr_neg_2 = 1 - tpr_neg_1 fpr_x_pos = z[2] / NM_pos fpr_x_neg = z[6] / NM_neg tpr_x_pos = z[0] / M_pos tpr_x_neg = z[4] / M_neg constraints = [ z >= 0, z <= 1, sum(z) == 1, # simplex constraint A_const @ z - b_const.flatten() == 0, # marginal sum const cp.sum(cp.abs(A @ z - b.flatten())) <= eps, ] if fairtype == 'EOd': constraints += [ tpr_pos_2 / fpr_pos_2 * fpr_x_pos - tpr_x_pos >= 0, tpr_x_pos - tpr_pos_1 / tpr_pos_1 * fpr_x_pos >= 0, tpr_pos_1 / fpr_pos_1 * (fpr_x_pos - 1) + 1 - tpr_x_pos >= 0, tpr_x_pos - tpr_pos_2 / fpr_pos_2 * (fpr_x_pos - 1) - 1 >= 0, # feasibility for pos group tpr_neg_2 / fpr_neg_2 * fpr_x_neg - tpr_x_neg >= 0, tpr_x_neg - tpr_neg_1 / tpr_neg_1 * fpr_x_neg >= 0, tpr_neg_1 / fpr_neg_1 * (fpr_x_neg - 1) + 1 - tpr_x_neg >= 0, tpr_x_neg - tpr_neg_2 / fpr_neg_2 * (fpr_x_neg - 1) - 1 >= 0 # feasibility for neg group ] elif fairtype == 'DP': #TODO constraints += [ tpr_pos_2 / fpr_pos_2 * fpr_x_pos - tpr_x_pos >= 0, tpr_x_pos - tpr_pos_1 / tpr_pos_1 * fpr_x_pos >= 0, tpr_pos_1 / fpr_pos_1 * (fpr_x_pos - 1) + 1 - tpr_x_pos >= 0, tpr_x_pos - tpr_pos_2 / fpr_pos_2 * (fpr_x_pos - 1) - 1 >= 0, # feasibility for pos group tpr_neg_2 / fpr_neg_2 * fpr_x_neg - tpr_x_neg >= 0, tpr_x_neg - tpr_neg_1 / tpr_neg_1 * fpr_x_neg >= 0, tpr_neg_1 / fpr_neg_1 * (fpr_x_neg - 1) + 1 - tpr_x_neg >= 0, tpr_x_neg - tpr_neg_2 / fpr_neg_2 * (fpr_x_neg - 1) - 1 >= 0 # feasibility for neg group ] else: pass solver = cp.ECOS objective = cp.Minimize(c @ z) prob = cp.Problem(objective, constraints) prob.solve(solver=solver, verbose=verbose) if verbose: print(prob.value) print(z.value) if plot: f, ax = plt.subplots() from matplotlib.patches import Polygon ax.add_patch( Polygon([[0, 0], [fpr_pos_1, tpr_pos_1], [1, 1], [fpr_pos_2, tpr_pos_2]], closed=True, color='r', alpha=0.3, label='A=1')) ax.add_patch( Polygon([[0, 0], [fpr_neg_1, tpr_neg_1], [1, 1], [fpr_neg_2, tpr_neg_2]], closed=True, color='b', alpha=0.3, label='A=0')) z_sol = z.value fpr_z_pos = z_sol[2] / (z_sol[3] + z_sol[2]) tpr_z_pos = z_sol[0] / (z_sol[0] + z_sol[1]) fpr_z_neg = z_sol[6] / (z_sol[6] + z_sol[7]) tpr_z_neg = z_sol[4] / (z_sol[4] + z_sol[5]) ax.scatter([fpr_z_pos], [tpr_z_pos], color='r', marker='x', label='classifeir') ax.scatter([fpr_z_neg], [tpr_z_neg], color='b', marker='x', label='classifier') ax.legend() ax.set_aspect('equal') ax.set_ylabel('TPR') ax.set_xlabel('FPR') return z_sol, prob.value return prob, z
def make_variable(shape): return cp.Variable(shape)
def LMM_contraction_F_G(L: float, mu: float, gamma: float, alpha: np.ndarray, beta: np.ndarray, epsilon: float, iteration=5, lower=0., upper=5., eps=0.): """ Compute the contraction rate for a quadratic Lyapunov Phi (G-stability), for a linear multistep method with two stages Phi(x_1 - y_1 )<= rho Phi(x_0 - y_0) Compute the associated Gram Matrix with lowest rank (trace minimization heuristic), and its function values Parameters ---------- L : float smoothness parameter for the class of functions mu : float strong-convexity parameter for the class of functions gamma : float step size of the method alpha : ndarray, shape (s,) internal step size beta : ndarray, shape (s,) external step size epsilon : float precision parameter for the binary search lower : float lower bound for binary search upper : float upper bound for binary search iterations : int Iterations k at which contraction is observed eps : float tolerance for non-violating the interpolation inequalities Returns ------- rho^2 : float contraction rate (square) G : np.ndarray , shape ((2+s)*2, (2+s)*2), Gram matrix F : np.ndarray, shape ((2+s)*2, ) function values """ ## PARAMETERS dimN = 2 # Norm in the Lyapunov dimG = 4 * 2 + 2 * iteration # dimension of the Gram matrix dimF = 2 * 2 + 2 * iteration npt = 2 * 2 + 2 * iteration # number of points ## INITIALIZE XX = [] FF = [] GG = [] YY = [] # Initial points x = np.zeros((1, dimG)) x[0][0] = 1. y = np.zeros((1, dimG)) y[0][2] = 1. x_ = np.zeros((1, dimG)) x_[0][1] = 1. y_ = np.zeros((1, dimG)) y_[0][3] = 1. XX.append(x) # x_10 XX.append(x_) # x_20 XX.append(y) # x_11 XX.append(y_) # x_21 # Construct the base for i in range(dimF): f = np.zeros((1, dimF)) g = np.zeros((1, dimG)) f[0][i] = 1. g[0][i + 4] = 1. FF.append(f) GG.append(g) ## ITERATES for i in range(4, dimG, 2): YY.append(XX[i - 4] * beta[0] + beta[1] * XX[i - 2]) # y_1i YY.append(XX[i - 3] * beta[0] + beta[1] * XX[i - 1]) # y_2i XX.append(-alpha[1] * XX[i - 2] - alpha[0] * XX[i - 4] - gamma * GG[i - 4]) # x_1i XX.append(-alpha[1] * XX[i - 1] - alpha[0] * XX[i - 3] - gamma * GG[i - 3]) # x_2i ## VECTOR FOR THE LYAPUNOV Y_11 = np.array([XX[-1], XX[-3]]) Y_12 = np.array([XX[-2], XX[-4]]) Y_01 = np.array([XX[-3], XX[-5]]) Y_02 = np.array([XX[-4], XX[-6]]) ## INTERPOLATION INEQUALITIES A = [] b = [] A_pos = [] b_pos = [] for i in range(npt): for j in range(npt): if j != i: Aij = np.dot((YY[i] - YY[j]).T, GG[j]) + \ 1 / 2 / (1 - mu / L) * (1 / L * np.dot((GG[i] - GG[j]).T, GG[i] - GG[j]) + mu * np.dot((YY[i] - YY[j]).T, YY[i] - YY[j]) - 2 * mu / L * np.dot((YY[i] - YY[j]).T, GG[i] - GG[j])) A.append(.5 * (Aij + Aij.T)) b.append(FF[j] - FF[i]) if (i <= npt - 3) & (j <= npt - 3): A_pos.append(.5 * (Aij + Aij.T)) b_pos.append(FF[j] - FF[i]) ## BINARY SEARCH # Updates value_N = None value_l = None while upper - lower >= epsilon: tau = (lower + upper) / 2 # VARIABLES N = cp.Variable((dimN, dimN), symmetric=True) l = cp.Variable((npt * (npt - 1), 1)) nu = cp.Variable(((npt - 1) * (npt - 2), 1)) # CONSTRAINTS ### Sign of interpolation inequalities constraints = [l <= 0.] ### Positivity of the Lyapunov function constraints = constraints + [nu <= 0.] constraints = constraints + [ (Y_01 - Y_02)[:, 0].T @ N @ (Y_01 - Y_02)[:, 0] - sum([nu[i][0] * A_pos[i] for i in range(len(A_pos))]) >> 0. ] constraints = constraints + [ -sum([nu[i][0] * b_pos[i][0] for i in range(len(b_pos))]) >= 0. ] ### Normalization of the generalized norm that defines the lyapunov function constraints = constraints + [cp.trace(N) == 1.] ### Interpolation inequalities constraints = constraints + [ sum([l[i][0] * A[i] for i in range(len(A))]) + ((Y_11 - Y_12)[:, 0].T @ N @ (Y_11 - Y_12)[:, 0]) - tau * ((Y_01 - Y_02)[:, 0].T @ N @ (Y_01 - Y_02)[:, 0]) << 0. ] constraints = constraints + [ sum([l[i][0] * b[i][0] for i in range(len(b))]) <= 0. ] ## OPTIMIZE prob = cp.Problem(cp.Minimize(0.), constraints) prob.solve(cp.SCS) if prob.status == 'optimal': upper = tau value_N = N.value # lyapunov function value_l = l.value else: lower = tau ## COMPUTE F,G WITH LOW RANK (trace heuristic) given the contraction rate and the lyapunov function # VARIBALES F = cp.Variable((dimF, 1)) # Function values G = cp.Variable((dimG, dimG), symmetric=True) # Gram matrix # CONSTRAINTS ### positivity of the Gram Matrix constraints = [G >> 0.] ### positivity of the function (convex) constraints = constraints + [F >= 0.] ### interpolation inequalities for i in range(npt): for j in range(npt): if j != i: A = np.dot((YY[i] - YY[j]).T, GG[j]) + 1 / 2 / (1 - mu / L) * ( 1 / L * np.dot( (GG[i] - GG[j]).T, GG[i] - GG[j]) + mu * np.dot( (YY[i] - YY[j]).T, YY[i] - YY[j]) - 2 * mu / L * np.dot((YY[i] - YY[j]).T, GG[i] - GG[j])) A = .5 * (A + A.T) b = FF[j] - FF[i] # Small violation of the interpolation constraints while reconstructing constraints += [ b[0] @ F[:, 0] + sum([(A @ G)[k, k] for k in range(dimG)]) <= -eps ] ### Fix the value of the previous step constraints = constraints + [ cp.trace( ((Y_01 - Y_02)[:, 0].T @ value_N @ (Y_01 - Y_02)[:, 0]) @ G) == 1. ] ### Satisfy worst-case contraction rate constraints = constraints + [ cp.trace(((Y_11 - Y_12)[:, 0].T @ value_N @ (Y_11 - Y_12)[:, 0]) @ G) == tau * cp.trace( ((Y_01 - Y_02)[:, 0].T @ value_N @ (Y_01 - Y_02)[:, 0]) @ G) ] ## OPTIMIZE prob = cp.Problem(cp.Minimize(cp.trace(G)), constraints) prob.solve(cp.SCS) value_G = G.value ## VERIFY THE CONTRACTION RATE #trace1 = np.trace(((Y_11 - Y_12)[:, 0].T @ value_N @ (Y_11 - Y_12)[:, 0]) @ value_G ) #print(trace1) #trace2 = np.trace(((Y_01 - Y_02)[:, 0].T @ value_N @ (Y_01 - Y_02)[:, 0]) @ value_G) #print(trace2) #print(trace1/trace2) return F.value, G.value, tau, value_N, value_l
def main(mat): amat = mat['A'] bmat = mat['B'] print('Input shape:', amat.shape) n = amat.shape[0] qap_func = gen_qap_func(amat, bmat) qap_fhats = {(n,): np.array([1])} variables = {(n,): cp.Variable((1, 1))} #qap_irreps = [(n - 1, 1), (n - 2, 2), (n - 2, 1, 1)] qap_irreps = [(n - 1, 1), (n - 2, 1, 1)] for irrep in [(n - 1, 1), (n - 2, 2), (n - 2, 1, 1)]: qhat = qap_fhat(amat, bmat, irrep) qap_fhats[irrep] = qhat variables[irrep] = make_variable(qhat.shape) c_lambdas = {} c_lambdas_inv = {} for irrep in [(n - 1, 1), (n - 2, 1, 1)]: c = c_lambda(irrep) c_lambdas_inv[irrep] = np.linalg.inv(c) c_lambdas[irrep] = c print(irrep, np.linalg.norm(c), np.allclose([email protected], np.eye(len(c)))) pdb.set_trace() # These need to be functions of variables block_diags = {} #for irrep in qap_irreps: #for irrep in [(n - 1, 1), (n - 2, 2), (n - 2, 1, 1)]: for irrep in [(n - 1, 1), (n - 2, 1, 1)]: birreps, _ = qap_decompose(irrep) print('irrep: {} | decompose: {}'.format(irrep, birreps)) block_diags[irrep] = gen_block_diag_vars(variables, birreps) print('done irrep: {} | decompose: {}'.format(irrep, birreps)) # constraints are functions of the block_diags d_n = hook_length((n,)) / math.factorial(n) d_n1 = hook_length((n - 1, 1)) / math.factorial(n) d_n22 = hook_length((n - 2, 2)) / math.factorial(n) d_n211 = hook_length((n - 2, 1, 1)) / math.factorial(n) obj = \ d_n * cp.sum(cp.multiply(variables[(n,)], qap_fhats[(n,)])) + \ d_n1 * cp.sum(cp.multiply(variables[(n - 1, 1)], qap_fhats[(n - 1, 1)])) + \ d_n22 * cp.sum(cp.multiply(variables[(n - 2, 2)], qap_fhats[(n - 2, 2)])) + \ d_n211 * cp.sum(cp.multiply(variables[(n - 2, 1, 1)], qap_fhats[(n - 2, 1, 1)])) n1_one = np.ones((block_diags[(n-1, 1)].shape[0], 1)) n2_one = np.ones((block_diags[(n-2, 1, 1)].shape[0], 1)) constraints = [ #c_lambdas_inv[(n - 1, 1)] @ block_diags[(n - 1, 1)] @ c_lambdas[(n - 1, 1)] >= 0, #(c_lambdas_inv[(n - 1, 1)] @ block_diags[(n - 1, 1)] @ c_lambdas[(n - 1, 1)]) @ n1_one == 1, #(c_lambdas_inv[(n - 1, 1)] @ block_diags[(n - 1, 1)] @ c_lambdas[(n - 1, 1)]).T @ n1_one == 1, # c_lambdas_inv[(n - 2, 2)] @ block_diags[(n - 2, 2)] @ c_lambdas[(n - 2, 2)] >= 0, c_lambdas_inv[(n - 2, 1, 1)] @ block_diags[(n - 2, 1, 1)] @ c_lambdas[(n - 2, 1, 1)] >= 0, #c_lambdas_inv[(n - 2, 1, 1)] @ block_diags[(n - 2, 1, 1)] @ c_lambdas[(n - 2, 1, 1)] @ n2_one == 1, #(c_lambdas_inv[(n - 2, 1, 1)] @ block_diags[(n - 2, 1, 1)] @ c_lambdas[(n - 2, 1, 1)]).T @ n2_one == 1, variables[(n,)] == 1 ] print(len(constraints)) problem = cp.Problem(cp.Maximize(obj), constraints) #problem.solve(solver=cp.OSQP) result =problem.solve(solver=cp.OSQP, verbose=True) #print(f'Obj: {problem.value:.2f}') perm = c_lambdas_inv[(n-1, 1)] @ block_diags[(n - 1, 1)].value @c_lambdas[(n-1, 1)] perm_tup = c_lambdas_inv[(n-2, 1, 1)] @ block_diags[(n - 2, 1, 1)].value @c_lambdas[(n-2, 1, 1)] res = ((perm @ amat @ perm.T)@ bmat).sum() print('Perm tup sums:', perm_tup.sum(axis=1), perm_tup.sum(axis=0), perm_tup.sum(), perm_tup.shape) print(f'Lower bound: {res} | result: {result}') pdb.set_trace()
def __init__(self, size): self._bool_var_size = size self._cvxpy_var = cvxpy.Variable(rows=2**size)
# Generate data for long only portfolio optimization. import numpy as np np.random.seed(1) n = 10 mu = np.abs(np.random.randn(n, 1)) Sigma = np.random.randn(n, n) Sigma = Sigma.T.dot(Sigma) # Long only portfolio optimization. import cvxpy as cp w = cp.Variable(n) gamma = cp.Parameter(nonneg=True) ret = mu.T*w risk = cp.quad_form(w, Sigma) prob = cp.Problem(cp.Maximize(ret - gamma*risk), [cp.sum(w) == 1, w >= 0]) # Compute trade-off curve. SAMPLES = 10000 risk_data = np.zeros(SAMPLES) ret_data = np.zeros(SAMPLES) gamma_vals = np.logspace(-2, 3, num=SAMPLES) for i in range(SAMPLES): gamma.value = gamma_vals[i] prob.solve() risk_data[i] = cp.sqrt(risk).value ret_data[i] = ret.value
def test_CBC_hard(self): num_states = 5 x = cvxpy.Bool(num_states,name='x') sw_on = cvxpy.Bool(num_states,name='sw_on') sw_off = cvxpy.Bool(num_states,name='sw_off') sw_stay_on = cvxpy.Bool(num_states,name='sw_stay_on') sw_stay_off = cvxpy.Bool(num_states,name='sw_stay_off') fl = cvxpy.Variable(num_states,name='float') constr = [] # can only be one transition type constr.append(sw_on*1.0 + sw_off*1.0 + sw_stay_on*1.0 + sw_stay_off*1.0 == 1) for i in range(num_states): # if switching on, must be now on constr.append(x[i] >= sw_on[i]) # if switchin on, must have been off previously if i>0: constr.append((1-x[i-1]) >= sw_on[i]) # if switching off, must be now off constr.append((1-x[i]) >= sw_off[i]) # if switchin off, must have been on previously if i>0: constr.append(x[i-1] >= sw_off[i]) # if staying on, must be now on constr.append(x[i] >= sw_stay_on[i]) # if staying on, must have been on previously if i>0: constr.append(x[i-1] >= sw_stay_on[i]) # if staying, must be now off constr.append((1-x[i]) >= sw_stay_off[i]) # if staying off, must have been off previously if i>0: constr.append((1-x[i-1]) >= sw_stay_off[i]) # random stuff constr.append(x[1] == 1) constr.append(x[3] == 0) for i in range(num_states): constr.append(fl[i] <= i*sw_on[i]) constr.append(fl[i] >= -i) obj = cvxpy.Maximize(sum(sw_off) + sum(sw_on)) for i in range(num_states): if i%2 == 0: obj += cvxpy.Maximize(fl[i]) else: obj += cvxpy.Maximize(-1*fl[i]) problem = cvxpy.Problem(obj, constr) ret = problem.solve(solver=cvxpy.CBC) self.assertTrue(problem.status in [cvxpy.OPTIMAL, cvxpy.OPTIMAL_INACCURATE]) print(' | '.join(['i','x','sw_on','sw_off','stay_on','stay_off','float'])) for i in range(num_states): row = ' | '.join([ '%1d' % i, '%1d' % int(round(x[i].value)), '%5d' % int(round(sw_on[i].value)), '%6d' % int(round(sw_off[i].value)), '%7d' % int(round(sw_stay_on[i].value)), '%8d' % int(round(sw_stay_off[i].value)), '%f' % fl[i].value ]) print(row)
def MPC_calc(csp, mpc_est, ITERATION, track_info): mpcp = MPC_path() if ITERATION == 0: # track info s_track = track_info.s c = track_info.c yaw = track_info.yaw #initial position x_0 scar = mpc_est.scar #Starta på s=0 måste vara ok! # optimal cost deltas s = mpc_est.s[0:(N) + 1] #states psi_bar = mpc_est.e_psi[0:(N) + 1] psi = mpc_est.e_psi[0] d_bar = mpc_est.d[0:(N) + 1] d = mpc_est.d[0] v_bar = mpc_est.v[0:(N) + 1] v = mpc_est.v[0] K_bar = mpc_est.K[0:(N) + 1] K = K_bar[0] a_bar = mpc_est.a[0:(N) + 1] a = a_bar[0] #inputs C_bar = mpc_est.C[0:(N - 1) + 1] C = K_bar[0] J_bar = mpc_est.J[0:(N - 1) + 1] J = J_bar[0] #HIT ÄR ALLT BRA! else: #track info s_track = track_info.s c = track_info.c yaw = track_info.yaw #states only the actual state needed to predict future x and u's scar = mpc_est.scar psi = mpc_est.psi[1] d = mpc_est.d[1] v = mpc_est.v[1] K = mpc_est.K[1] a = mpc_est.a[1] C = mpc_est.C[1] J = mpc_est.J[1] #The reference/privious prediction vectors s = mpc_est.s[0:(N) + 1] psi_bar = mpc_est.psi[1:(N) + 1] d_bar = mpc_est.d[1:(N) + 1] v_bar = mpc_est.v[1:(N) + 1] K_bar = mpc_est.K[1:(N) + 1] a_bar = mpc_est.a[1:(N) + 1] C_bar = mpc_est.C[1:(N - 1) + 1] #skall dessa framåt? J_bar = mpc_est.J[1:(N - 1) + 1] print "REFERENS psi:", psi_bar print "REFERENS d:", d_bar print "REFERENS v:", v_bar print "REFERENS K:", K_bar print "REFERENS a:", a_bar print "REFERENS s:", s X_0 = [d, psi, v, K, a] U_0 = [C, J] x_bar_vec = np.matrix([d_bar, psi_bar, v_bar, K_bar, a_bar]) u_bar_vec = np.matrix([C_bar, J_bar]) n = 5 # States x m = 2 # Control signals u print "X_0:", X_0 print "U_0:", U_0 x = cvxpy.Variable(n, N) u = cvxpy.Variable(m, N - 1) slack = cvxpy.Variable(n, N) slackC = cvxpy.Variable(1, 1) slackJ = cvxpy.Variable(1, 1) cost_matrix = np.eye(n, n) Q_slack = cost_matrix * 1 Q_inputs = 1 cost = 0.0 constr = [] c_bar = [] for t in range( N - 1): #Detta är som att köra MPCn fär alla N states, är de korrekt? x_bar = x_bar_vec[:, t] u_bar = u_bar_vec[:, t] s_aprox = s[t] #Mpc estimerade s punkterna s_point = s_aprox idx = (np.abs(np.asarray(s_track) - s_point)).argmin() c_bar.append(c[idx]) yaw_prime = (yaw[idx + 1] - yaw[idx] ) / 0.05 #ds = 0.05 from csp. I teorin samma som c_bar #print "is yaw prime -1/R?:", yaw_prime, "c_bar:", c_bar[-1] #TESTAT byta ut K_bar mot yaw_prime för att inte vara beroende av mina gissningar. F = np.matrix([[(1 - d_bar[t] * c_bar[-1]) * math.tan(psi_bar[t])], [((1 - d_bar[t] * c_bar[-1]) * K_bar[t]) / (math.cos(psi_bar[t])) - yaw_prime], [((1 - d_bar[t] * c_bar[-1]) * a_bar[t]) / (v_bar[t] * math.cos(psi_bar[t]))], [((1 - d_bar[t] * c_bar[-1]) * C_bar[t]) / (v_bar[t] * math.cos(psi_bar[t]))], [((1 - d_bar[t] * c_bar[-1]) * J_bar[t]) / (v_bar[t] * math.cos(psi_bar[t]))]]) A = np.matrix( [[ -c_bar[-1] * math.tan(psi_bar[t]), ((1 - d_bar[t] * c_bar[-1])) / (math.cos(psi_bar[t])**2), 0, 0, 0 ], [ -(c_bar[-1] * K_bar[t]) / (math.cos(psi_bar[t])), (((1 - d_bar[t] * c_bar[-1]) * K_bar[t]) * math.tan(psi_bar[t])) / (math.cos(psi_bar[t])), 0, (1 - d_bar[t] * c_bar[-1]) / math.cos(psi_bar[t]), 0 ], [ -(c_bar[-1] * a_bar[t]) / (v_bar[t] * math.cos(psi_bar[t])), ((1 - d_bar[t] * c_bar[-1]) * a_bar[t] * math.tan(psi_bar[t])) / (v_bar[t] * math.cos(psi_bar[t])), -((1 - d_bar[t] * c_bar[-1]) * a_bar[t]) / (v_bar[t]**2 * math.cos(psi_bar[t])), 0, (1 - d_bar[t] * c_bar[-1]) / (v_bar[t] * math.cos(psi_bar[t])) ], [ -(c_bar[t] * C_bar[t]) / (v_bar[t] * math.cos(psi_bar[t])), ((1 - d_bar[t] * c_bar[-1]) * C_bar[t] * math.tan(psi_bar[t])) / (v_bar[t] * math.cos(psi_bar[t])), -((1 - d_bar[t] * c_bar[-1]) * C_bar[t]) / (v_bar[t]**2 * math.cos(psi_bar[t])), 0, 0 ], [ -(c_bar[t] * J_bar[t]) / (v_bar[t] * math.cos(psi_bar[t])), ((1 - d_bar[t] * c_bar[-1]) * J_bar[t] * math.tan(psi_bar[t])) / (v_bar[t] * math.cos(psi_bar[t])), -((1 - d_bar[t] * c_bar[-1]) * J_bar[t]) / (v_bar[t]**2 * math.cos(psi_bar[t])), 0, 0 ]]) invert = is_invertible(A) print "Invertable?: ", invert #Ad = np.eye(n, n) + (DT*A) B = np.matrix([[0, 0], [0, 0], [ 0, 0 ], [ (1 - d_bar[t] * c_bar[-1]) / (v_bar[t] * math.cos(psi_bar[t])), 0 ], [0, (1 - d_bar[t] * c_bar[-1]) / (v_bar[t] * math.cos(psi_bar[t]))]]) M_matrix = np.block([[A, B], [np.eye(m, n) * 0, np.eye(m, m) * 0]]) M_matrix = DT * M_matrix M = scipy.linalg.expm(M_matrix) Ad = M[:n, :n] Bd = M[:n, n:] #Pedro + Max cost function cost += -(c_bar[-1]) / (v_bar[t]) * x[0, t] cost += (1 - d_bar[t] * c_bar[-1]) / (2 * v_bar[t]) * x[1, t]**2 cost += -((1 - d_bar[t] * c_bar[-1])) / (v_bar[t]**2) * x[2, t] cost += sum_squares(Q_slack * slack[:, t]) cost += slackC**2 * Q_inputs cost += slackJ**2 * Q_inputs constr += [ x[:, t + 1] == Ad * x[:, t] + Bd * u[:, t] + F - A * x_bar - B * u_bar ] #se till att implementera x_bar rätt constr += [x[0, t + 1] <= MAX_ROAD_WIDTH + slack[0, t] ] #Lateral Deviation e_y constr += [x[0, t + 1] >= -MAX_ROAD_WIDTH - slack[0, t]] constr += [x[1, t + 1] <= MAX_PSI + slack[1, t] ] #Angular deviation e_psi constr += [x[1, t + 1] >= -MAX_PSI - slack[1, t]] constr += [x[2, t + 1] <= MAX_SPEED + slack[2, t]] #Velocity v_x constr += [x[2, t + 1] >= 0 - slack[2, t]] constr += [x[3, t + 1] <= MAX_CURVATURE + slack[3, t] ] #Curvature K = tan(delta)/l constr += [x[3, t + 1] >= -MAX_CURVATURE - slack[3, t]] constr += [x[4, t + 1] <= MAX_ACCEL + slack[4, t]] #Acceleration a_x constr += [x[4, t + 1] >= -MAX_ACCEL - slack[4, t]] constr += [u[0, t] <= MAX_C + slackC] #Sharpness C constr += [u[0, t] >= -MAX_C - slackC] constr += [u[1, t] <= MAX_J + slackJ] #Longitudional Jerk constr += [u[1, t] >= -MAX_J - slackJ] #slack variables constr += [slack[:, t] >= 0] constr += [slackC >= 0] constr += [slackJ >= 0] #print("Constr is DCP:", (x[:, t + 1] == (A * x[:, t] + B * u[:, t])).is_dcp()) ''' print("Constr is DCP road with :", (x[0, t] <= MAX_ROAD_WIDTH).is_dcp(), "and: ", (x[0, t] >= -MAX_ROAD_WIDTH).is_dcp()) print("Constr is DCP angle error :", (x[1, t] <= MAX_PSI).is_dcp(), "and: ", (x[1, t] >= -math.pi/2).is_dcp()) print("Constr is DCP velocity :", (x[2, t] <= MAX_SPEED).is_dcp(), "and: ", (x[2, t] >= 0).is_dcp()) print("Constr is DCP Kurvature :", (x[3, t] <= MAX_CURVATURE).is_dcp(), "and: ", (x[3, t] >= -MAX_CURVATURE).is_dcp()) print("Constr is DCP Acceleration:", (x[4, t] <= MAX_ACCEL).is_dcp(), "and: ", (x[4, t] >= -MAX_ACCEL).is_dcp()) ''' constr += [x[:, 0] == X_0] #constr += [u[:, 0] == U_0] print("Constr is DCP:", (x[:, 0] == X_0).is_dcp()) prob = cvxpy.Problem(cvxpy.Minimize(cost), constr) print("prob is DCP:", prob.is_dcp()) print("Cost is DCP:", cost.is_dcp()) print("curvature of x:", x.curvature) print("curvature of u:", u.curvature) print("curvature of cost:", cost.curvature) prob.solve(verbose=False) print "Status:", prob.status print "Optimal value with ECOS:", prob.value if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE: #The outputs from the MPC is the real states mpcp.psi, mpcp.d, mpcp.v, mpcp.K, mpcp.a, mpcp.s = [], [], [], [], [], [] mpcp.d = x.value[0, :].tolist() mpcp.d = mpcp.d[0] mpcp.psi = x.value[1, :].tolist() #De riktiga statsen mpcp.psi = mpcp.psi[0] mpcp.v = x.value[2, :].tolist() mpcp.v = mpcp.v[0] mpcp.K = x.value[3, :].tolist() mpcp.K = mpcp.K[0] mpcp.a = x.value[4, :].tolist() mpcp.a = mpcp.a[0] mpcp.C = u.value[0, :].tolist() mpcp.C = mpcp.C[0] mpcp.J = u.value[1, :].tolist() mpcp.J = mpcp.J[0] #beräkningen av s är fel! Ända felet kvar som jag ser nu #lägger till de interpolerade sista statet mpcp.d.append(mpcp.d[-1]) mpcp.psi.append(mpcp.psi[-1]) mpcp.v.append(mpcp.v[-1]) mpcp.K.append(mpcp.K[-1]) mpcp.a.append(mpcp.a[-1]) mpcp.C.append(mpcp.C[-1]) mpcp.J.append(mpcp.J[-1]) #gör en egen loop för att få rätt S mpcp.s.append(scar) for l in range(N - 2): #mpcp.s.append(scar + ((DT/mpcp.v[l])*mpcp.v[l]*math.cos(mpcp.psi[l]))/(1 - mpcp.d[l]*c_bar[l])) scar = mpcp.s[-1] #mpcp.s.append(scar + ((DT/mpcp.v[-1]) * mpcp.v[-1] * math.cos(mpcp.psi[-1])) / (1 - mpcp.d[-1] * c_bar[-1])) #mpcp.s.append(scar + ((DT/mpcp.v[-1]) * mpcp.v[-1] * math.cos(mpcp.psi[-1])) / (1 - mpcp.d[-1] * c_bar[-1])) mpcp.s = s[1:] mpcp.s.append(mpcp.s[-1] + DT) mpcp.scar = mpcp.s[1] #Mycket verkar ok till hit! print "mpc solution psi:", mpcp.psi print "mpc solution d:", mpcp.d print "mpc solution v:", mpcp.v print "mpc solution K:", mpcp.K print "mpc solution a:", mpcp.a print "mpc solution s:", mpcp.s sd = slack.value[0, :] spsi = slack.value[1, :] sv = slack.value[2, :] sK = slack.value[3, :] sa = slack.value[4, :] slackC = slackC.value slackJ = slackJ.value print "The Slack d variable is:", sd print "The Slack psi variable is:", spsi print "The Slack v variable is:", sv print "The Slack K variable is:", sK print "The Slack a variable is:", sa print "The Slack C variable is:", slackC print "The Slack J variable is:", slackJ return mpcp
def compute_matrix_coloring(graph, sdp_type, verbose): """Finds matrix coloring M of graph using Mosek solver. Args: graph (nx.Graph): Graph to be processed. sdp_type (string): Non-strict, Strict or Strong vector coloring. verbose (bool): Sets verbosity level of solver. Returns: 2-dim matrix: Matrix coloring of graph G. Notes: Maybe we can add epsilon to SDP constraints instead of 'solve' parameters? For some reason optimal value of alpha is greater than value computed from M below if SDP is solved with big tolerance for error TODO: strong vector coloring """ logging.info( 'Computing matrix coloring of graph with {0} nodes and {1} edges...' .format(graph.number_of_nodes(), graph.number_of_edges())) result = None alpha_opt = None if config.solver_name == 'mosek': with Model() as Mdl: # Variables n = graph.number_of_nodes() alpha = Mdl.variable(Domain.lessThan(0.)) m = Mdl.variable(Domain.inPSDCone(n)) if n <= 50: sdp_type = 'strong' # Constraints Mdl.constraint(m.diag(), Domain.equalsTo(1.0)) for i in range(n): for j in range(n): if i > j and has_edge_between_ith_and_jth(graph, i, j): if sdp_type == 'strict' or sdp_type == 'strong': Mdl.constraint(Expr.sub(m.index(i, j), alpha), Domain.equalsTo(0.)) elif sdp_type == 'nonstrict': Mdl.constraint(Expr.sub(m.index(i, j), alpha), Domain.lessThan(0.)) elif i > j and sdp_type == 'strong': Mdl.constraint(Expr.add(m.index(i, j), alpha), Domain.greaterThan(0.)) # Objective Mdl.objective(ObjectiveSense.Minimize, alpha) # Set solver parameters # Mdl.setSolverParam("intpntCoTolRelGap", 1e-4) # Mdl.setSolverParam("intpntCoTolPfeas", 1e-5) # Mdl.setSolverParam("intpntCoTolMuRed", 1e-5) # Mdl.setSolverParam("intpntCoTolInfeas", 1e-7) # Mdl.setSolverParam("intpntCoTolDfeas", 1e-5) # mosek_params = { # 'MSK_DPAR_INTPNT_CO_TOL_REL_GAP': 1e-4, # 'MSK_DPAR_INTPNT_CO_TOL_PFEAS': 1e-5, # 'MSK_DPAR_INTPNT_CO_TOL_MU_RED': 1e-5, # 'MSK_DPAR_INTPNT_CO_TOL_INFEAS': 1e-7, # 'MSK_DPAR_INTPNT_CO_TOL_DFEAS': 1e-5, # 'MSK_DPAR_SEMIDEFINITE_TOL_APPROX': 1e-10 # } # with open(config.logs_directory() + 'logs', 'w') as outfile: if verbose: Mdl.setLogHandler(sys.stdout) # moze stworz jeden model na caly algorytm i tylko usuwaj ograniczenia? Mdl.solve() alpha_opt = alpha.level()[0] level = m.level() result = [[level[j * n + i] for i in range(n)] for j in range(n)] result = np.array(result) else: n = graph.number_of_nodes() # I must be doing something wrong with the model definition - too many constraints and variables # Variables alpha = cvxpy.Variable() Mat = cvxpy.Variable((n, n), PSD=True) # Constraints (can be done using trace as well) constraints = [] for i in range(n): constraints += [Mat[i, i] == 1] for i in range(n): for j in range(n): if i > j and has_edge_between_ith_and_jth(graph, i, j): constraints += [Mat[i, j] <= alpha] # Objective objective = cvxpy.Minimize(alpha) # Create problem instance problem = cvxpy.Problem(objective, constraints) # Solve mosek_params = { 'MSK_DPAR_INTPNT_CO_TOL_REL_GAP': 1e-4, 'MSK_DPAR_INTPNT_CO_TOL_PFEAS': 1e-5, 'MSK_DPAR_INTPNT_CO_TOL_MU_RED': 1e-5, 'MSK_DPAR_INTPNT_CO_TOL_INFEAS': 1e-7, 'MSK_DPAR_INTPNT_CO_TOL_DFEAS': 1e-5, 'MSK_DPAR_SEMIDEFINITE_TOL_APPROX': 1e-10 } mosek_params_default = { 'MSK_DPAR_INTPNT_CO_TOL_REL_GAP': 1e-7, 'MSK_DPAR_INTPNT_CO_TOL_PFEAS': 1e-8, 'MSK_DPAR_INTPNT_CO_TOL_MU_RED': 1e-8, 'MSK_DPAR_INTPNT_CO_TOL_INFEAS': 1e-10, 'MSK_DPAR_INTPNT_CO_TOL_DFEAS': 1e-8, 'MSK_DPAR_SEMIDEFINITE_TOL_APPROX': 1e-10 } try: problem.solve(solver=cvxpy.MOSEK, verbose=config.solver_verbose, warm_start=True, mosek_params=mosek_params) alpha_opt = alpha.value result = Mat.value except cvxpy.error.SolverError: print('\nerror in mosek, changing to cvxopt\n') problem.solve(solver=cvxpy.CVXOPT, verbose=config.solver_verbose, warm_start=True) alpha_opt = alpha.value result = Mat.value logging.info('Found matrix {0}-coloring'.format(1 - 1 / alpha_opt)) return result
def global_transport(o_t, d_t, gridsKey, grids_to_id, k, w): indice_count = np.zeros((_N, ), dtype=int) for j in range(_N): hex_cal = Calculator() hex_cal.SetLayer(12) key = gridsKey[j] indices = [j] for i in range(k): bhex_ids = hex_cal.HexCellBoudary(key, i + 1) inter = set(gridsKey).intersection(bhex_ids) indices_add = [grids_to_id[x] for x in inter] if len(indices_add) > 0: indices = indices + indices_add indice_count[j] = len(indices) gamma = cvx.Variable((np.sum(indice_count), 1)) S = cvx.Variable((_N, 1)) C = np.ones((_N, 1)) A_1 = np.zeros((_N, np.sum(indice_count))) O_1 = np.zeros((_N, np.sum(indice_count))) cost = np.zeros((1, np.sum(indice_count))) for j in range(_N): hex_cal = Calculator() hex_cal.SetLayer(12) key = gridsKey[j] indices = [j] for i in range(k): bhex_ids = hex_cal.HexCellBoudary(key, i + 1) inter = set(gridsKey).intersection(bhex_ids) indices_add = [grids_to_id[x] for x in inter] if len(indices_add) > 0: indices = indices + indices_add A_1[j, np.sum(indice_count[:j]):np.sum(indice_count[:(j + 1)])] = 1 for p in range(int(indice_count[j])): cost[:, np.sum(indice_count[:j]) + p] = w * cost_norm[j, indices[p]] O_1[j, np.sum(indice_count[:j]) + p] = w * cost_norm[j, indices[p]] A_2 = np.zeros((_N, np.sum(indice_count))) O_2 = np.zeros((_N, np.sum(indice_count))) for j in range(_N): hex_cal = Calculator() hex_cal.SetLayer(12) key = gridsKey[j] indices = [j] for i in range(k): bhex_ids = hex_cal.HexCellBoudary(key, i + 1) inter = set(gridsKey).intersection(bhex_ids) indices_add = [grids_to_id[x] for x in inter] if len(indices_add) > 0: indices = indices + indices_add for p in indices: key = gridsKey[p] indices_1 = [p] for i in range(k): bhex_ids = hex_cal.HexCellBoudary(key, i + 1) inter = set(gridsKey).intersection(bhex_ids) indices_add = [grids_to_id[x] for x in inter] if len(indices_add) > 0: indices_1 = indices_1 + indices_add index = indices_1.index(j) A_2[j, np.sum(indice_count[:p]) + index] = 1 O_2[j, np.sum(indice_count[:p]) + index] = w * cost_norm[j, p] obj = cvx.Minimize(C.T * S + cost * gamma) constr = [ A_2 * gamma + S >= o_t.reshape(-1, 1), A_2 * gamma - S <= o_t.reshape(-1, 1), 0 <= gamma, 0 <= S, A_1 * gamma == d_t.reshape(-1, 1) ] prob = cvx.Problem(obj, constr) d = prob.solve(solver=cvx.GLPK) tmp = o_t.reshape(_N, 1) ratio = np.zeros((_N, )) tild_d = np.dot(A_2, gamma.value) - np.dot(O_2, gamma.value) tild_d[tild_d < 0] = 0 for i in range(_N): if (tmp[i, 0] == 0) & (tild_d[i, 0] == 0): ratio[i] = 1 elif tild_d[i, 0] == 0: ratio[i] = tmp[i, 0] / (tild_d[i, 0] + 1) else: ratio[i] = tmp[i, 0] / tild_d[i, 0] return d, ratio
def iter_dccp(self, max_iter, tau, mu, tau_max, solver, **kwargs): """ ccp iterations :param max_iter: maximum number of iterations in ccp :param tau: initial weight on slack variables :param mu: increment of weight on slack variables :param tau_max: maximum weight on slack variables :param solver: specify the solver for the transformed problem :return value of the objective function, maximum value of slack variables, value of variables """ # split non-affine equality constraints constr = [] for arg in self.constraints: if str(type(arg)) == "<class 'cvxpy.constraints.zero.Zero'>" and not arg.is_dcp(): constr.append(arg.expr.args[0]<=arg.expr.args[1]) constr.append(arg.expr.args[1]<=arg.expr.args[0]) else: constr.append(arg) obj = self.objective self = cvx.Problem(obj, constr) it = 1 converge = False # keep the values from the previous iteration or initialization previous_cost = float("inf") previous_org_cost = self.objective.value variable_pres_value = [] for var in self.variables(): variable_pres_value.append(var.value) # each non-dcp constraint needs a slack variable var_slack = [] for constr in self.constraints: if not constr.is_dcp(): var_slack.append(cvx.Variable(constr.size)) while it<=max_iter and all(var.value is not None for var in self.variables()): constr_new = [] # objective temp = convexify_obj(self.objective) if not self.objective.is_dcp(): # non-sub/super-diff while temp is None: # damping var_index = 0 for var in self.variables(): #var_index = self.variables().index(var) var.value = 0.8*var.value + 0.2* variable_pres_value[var_index] var_index += 1 temp = convexify_obj(self.objective) # domain constraints for dom in self.objective.args[0].domain: constr_new.append(dom) # new cost function cost_new = temp.args[0] # constraints count_slack = 0 for arg in self.constraints: temp = convexify_constr(arg) if not arg.is_dcp(): while temp is None: # damping for var in self.variables: var_index = self.variables().index(var) var.value = 0.8*var.value + 0.2* variable_pres_value[var_index] temp = convexify_constr(arg) newcon = temp[0] # new constraint without slack variable for dom in temp[1]:# domain constr_new.append(dom) right = newcon.expr.args[1] + var_slack[count_slack] constr_new.append(newcon.expr.args[0]<=right) constr_new.append(var_slack[count_slack]>=0) count_slack = count_slack+1 else: constr_new.append(temp) # objective if self.objective.NAME == 'minimize': for var in var_slack: cost_new += tau*cvx.sum(var) obj_new = cvx.Minimize(cost_new) else: for var in var_slack: cost_new -= tau*cvx.sum(var) obj_new = cvx.Maximize(cost_new) # new problem prob_new = cvx.Problem(obj_new, constr_new) # keep previous value of variables variable_pres_value = [] for var in self.variables(): variable_pres_value.append(var.value) # solve if solver is None: logger.info("iteration=%d, cost value=%.5f, tau=%.5f", it, prob_new.solve(**kwargs), tau) else: logger.info("iteration=%d, cost value=%.5f, tau=%.5f", it, prob_new.solve(solver=solver, **kwargs), tau) max_slack = None # print slack if (prob_new._status == "optimal" or prob_new._status == "optimal_inaccurate") and not var_slack == []: slack_values = [v.value for v in var_slack if v.value is not None] max_slack = max([np.max(v) for v in slack_values] + [-np.inf]) logger.info("max slack = %.5f", max_slack) #terminate if np.abs(previous_cost - prob_new.value) <= 1e-3 and np.abs(self.objective.value - previous_org_cost) <= 1e-3: it_real = it it = max_iter+1 converge = True else: previous_cost = prob_new.value previous_org_cost = self.objective.value it_real = it tau = min([tau*mu,tau_max]) it += 1 # return if converge: self._status = "Converged" else: self._status = "Not_converged" var_value = [] for var in self.variables(): var_value.append(var.value) if not var_slack == []: return(self.objective.value, max_slack, var_value) else: return(self.objective.value, var_value)
def relaxed_qclp(adj, alpha, fragile, local_budget, reward, teleport, global_budget=None, upper_bounds=None): """ Solves the linear program associated with the relaxed QCLP. Parameters ---------- adj : sp.spmatrix, shape [n, n] Sparse adjacency matrix. alpha : float (1-alpha) teleport[v] is the probability to teleport to node v. fragile : np.ndarray, shape [?, 2] Fragile edges that are under our control. local_budget : np.ndarray, shape [n] Maximum number of local flips per node. reward : np.ndarray, shape [n] Reward vector. teleport : np.ndarray, shape [n] Teleport vector. global_budget : int Global budget. upper_bounds : np.ndarray, shape [n] Upper bound for the values of x_i. Returns ------- xval : np.ndarray, shape [n+len(fragile)] The value of the decision variables. opt_fragile : np.ndarray, shape [?, 2] Optimal fragile edges. obj_value : float Optimal objective value. """ n = adj.shape[0] n_fragile = len(fragile) n_states = n + n_fragile adj = adj.copy() adj_clean = adj.copy() # turn off all existing edges before starting adj = adj.tolil() adj[fragile[:, 0], fragile[:, 1]] = 0 # add an edge from the source node to the new auxiliary variables source_to_aux = sp.lil_matrix((n, n_fragile)) source_to_aux[fragile[:, 0], np.arange(n_fragile)] = 1 original_nodes = sp.hstack((adj, source_to_aux)) original_nodes = sp.diags(1 / original_nodes.sum(1).A1) @ original_nodes # transitions among the original nodes are discounted by alpha original_nodes[:, :n] *= alpha # add an edge from the auxiliary variables back to the source node aux_to_source = sp.lil_matrix((n_fragile, n)) aux_to_source[np.arange(n_fragile), fragile[:, 0]] = 1 turned_off = sp.hstack((aux_to_source, sp.csr_matrix( (n_fragile, n_fragile)))) # add an edge from the auxiliary variables to the destination node aux_to_dest = sp.lil_matrix((n_fragile, n)) aux_to_dest[np.arange(n_fragile), fragile[:, 1]] = 1 turned_on = sp.hstack((aux_to_dest, sp.csr_matrix((n_fragile, n_fragile)))) # transitions from aux nodes when turned on are discounted by alpha turned_on *= alpha trans = sp.vstack((original_nodes, turned_off, turned_on)).tocsr() states = np.arange(n + n_fragile) states = np.concatenate((states, states[-n_fragile:])) c = np.zeros(len(states)) # reward for the original nodes c[:n] = reward # negative reward if we are going back to the source node c[n:n + n_fragile] = -reward[fragile[:, 0]] one_hot = sp.eye(n_states).tocsr() A = one_hot[states] - trans b = np.zeros(n_states) b[:n] = (1 - alpha) * teleport x = cp.Variable(len(c), nonneg=True) # set up the sums of auxiliary variables for local and global budgets frag_adj = sp.lil_matrix((len(c), len(c))) # the indices of the turned off/on auxiliary nodes idxs_off = n + np.arange(n_fragile) idxs_on = n + n_fragile + np.arange(n_fragile) # if the edge exists in the clean graph use the turned off node, otherwise the turned on node exists = adj_clean[fragile[:, 0], fragile[:, 1]].A1 idx_off_on_exists = np.where(exists, idxs_off, idxs_on) # each source node is matched with the correct auxiliary node (off or on) frag_adj[fragile[:, 0], idx_off_on_exists] = 1 deg = (trans != 0).sum(1).A1 unique = np.unique(fragile[:, 0]) # the local budget constraints are sum_i ( x_i_{on/off} * deg_i ) <= budget * x_i) # we index only on the unique source nodes to avoid trivial constraints budget_constraints = [ cp.multiply((frag_adj @ x)[unique], deg[unique]) <= cp.multiply( local_budget[unique], x[unique]) ] if global_budget is not None and upper_bounds is not None: # if we have a bounds matrix (for any PPR vector) we need to compute the upper bounds for the teleport if len(upper_bounds.shape) == 2: upper_bounds = teleport @ upper_bounds # do not consider upper_bounds that are zero nnz_unique = unique[upper_bounds[unique] != 0] # the global constraint is sum_i ( x_i_{on/off} * deg_i / upper(x_i) ) <= budget ) global_constraint = [ (frag_adj @ x)[nnz_unique] @ (deg[nnz_unique] / upper_bounds[nnz_unique]) <= global_budget ] else: if global_budget is not None or upper_bounds is not None: warnings.warn( 'Either global_budget or upper_bounds is provided, but not both. ' 'Solving using only local budget.') global_constraint = [] prob = cp.Problem(objective=cp.Maximize(c * x), constraints=[x * A == b] + budget_constraints + global_constraint) prob.solve(solver='GUROBI', verbose=False) assert prob.status == 'optimal' xval = x.value # reshape the decision variables such that x_ij^0 and x_ij^1 are in the same row opt_fragile_on_off = xval[n:].reshape(2, -1).T.argmax(1) opt_fragile = fragile[opt_fragile_on_off != exists] obj_value = prob.value return xval, opt_fragile, obj_value, prob
def __init__(self, pos_node, neg_node, resistance): self._current = cp.Variable() self.resistance = resistance super(Resistor, self).__init__(pos_node, neg_node)
def test_basic_correctness(basic_init_fixture, solver_name, i, H, g, x_ineq): """Basic test case for solver wrappers. The input fixture `basic_init_fixture` has two constraints, one velocity and one acceleration. Hence, in this test, I directly formulate an optimization with cvxpy and compare the result with the result obtained from the solver wrapper. """ constraints, path, path_discretization, vlim, alim = basic_init_fixture if solver_name == "cvxpy": from toppra.solverwrapper.cvxpy_solverwrapper import cvxpyWrapper solver = cvxpyWrapper(constraints, path, path_discretization) elif solver_name == 'qpOASES': from toppra.solverwrapper.qpoases_solverwrapper import qpOASESSolverWrapper solver = qpOASESSolverWrapper(constraints, path, path_discretization) elif solver_name == 'hotqpOASES': from toppra.solverwrapper.hot_qpoases_solverwrapper import hotqpOASESSolverWrapper solver = hotqpOASESSolverWrapper(constraints, path, path_discretization) elif solver_name == 'ecos' and H is None: from toppra.solverwrapper.ecos_solverwrapper import ecosWrapper solver = ecosWrapper(constraints, path, path_discretization) elif solver_name == 'seidel' and H is None: from toppra.solverwrapper.cy_seidel_solverwrapper import seidelWrapper solver = seidelWrapper(constraints, path, path_discretization) else: return True # Skip all other tests xmin, xmax = x_ineq xnext_min = 0 xnext_max = 1 # Results from solverwrapper to test solver.setup_solver() result_ = solver.solve_stagewise_optim(i - 2, H, g, xmin, xmax, xnext_min, xnext_max) result_ = solver.solve_stagewise_optim(i - 1, H, g, xmin, xmax, xnext_min, xnext_max) solverwrapper_result = solver.solve_stagewise_optim( i, H, g, xmin, xmax, xnext_min, xnext_max) solver.close_solver() # Results from cvxpy, used as the actual, desired values ux = cvxpy.Variable(2) u = ux[0] x = ux[1] _, _, _, _, _, _, xbound = solver.params[0] # vel constraint a, b, c, F, h, ubound, _ = solver.params[1] # accel constraint a2, b2, c2, F2, h2, _, _ = solver.params[2] # random constraint Di = path_discretization[i + 1] - path_discretization[i] v = a[i] * u + b[i] * x + c[i] v2 = a2[i] * u + b2[i] * x + c2[i] cvxpy_constraints = [ x <= xbound[i, 1], x >= xbound[i, 0], F * v <= h, F2[i] * v2 <= h2[i], x + u * 2 * Di <= xnext_max, x + u * 2 * Di >= xnext_min, ] if not np.isnan(xmin): cvxpy_constraints.append(x <= xmax) cvxpy_constraints.append(x >= xmin) if H is not None: objective = cvxpy.Minimize(0.5 * cvxpy.quad_form(ux, H) + g * ux) else: objective = cvxpy.Minimize(g * ux) problem = cvxpy.Problem(objective, cvxpy_constraints) problem.solve(verbose=True) # test with the same solver as cvxpywrapper if problem.status == "optimal": cvxpy_result = np.array(ux.value).flatten() solverwrapper_result = np.array(solverwrapper_result).flatten() npt.assert_allclose(solverwrapper_result, cvxpy_result, atol=5e-2, rtol=1e-5) # Very bad accuracy? why? else: assert np.all(np.isnan(solverwrapper_result))