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