def idr(y,
X,
groups=None,
orders=dict({"1": "comp"}),
verbose=False,
max_iter=10000,
eps_rel=0.00001,
eps_abs=0.00001):
"""
Fits isotonic distirbutional regression (IDR) to a training dataset.
Parameters
----------
y : np.array
one dimensional array (response variable)
X : pd.DataFrame
data frame of variables (regression covariates)
groups : dictionary
denoting groups of variables that are to be ordered with the same order.
Only relevant if X contains more than one variable
The default uses one group for all variables
orders : dictionary
denotes the order that is applied to a group.
Only relevant if X contains mode than one variable
The default is dict({"1":"comp"}).
(OSQP Solver Setting)
verbose : boolean
print output of OSQP solver. The default is False
max_iter : maximum number of iterations
eps_rel : relative tolerance
epc_abs : absolute tolerance
Returns
-------
An object of class idrobject containing the components:
X: data frame of distinct covariate combinations used for fit
y: list of responses for given covariate combination
cdf: estimated CDF
thresholds: where CDF is evaluated
groups: groups used for estimation
orders: orders used for estimation
indices: indices of covariates in original dataset
constraints: in multivariate IDR is None,
otherwise the order constraints for optimization
"""
if not isinstance(X, pd.DataFrame):
raise ValueError("X must be a pandas data frame")
if groups is None:
groups = dict(zip(X.columns, np.ones(X.shape[1])))
y = np.asarray(y)
if y.ndim > 1:
raise ValueError('idr only handles 1-D arrays of observations')
if isinstance(X, pd.DataFrame) == False:
raise ValueError("data must be a pandas data frame")
if X.shape[0] <= 1:
raise ValueError('X must have more than 1 row')
if np.isnan(np.sum(y)) == True:
raise ValueError("y contains nan values")
if X.isnull().values.any() == True:
raise ValueError("X contains nan values")
if y.size != X.shape[0]:
raise ValueError("length of y must match number of rows in X")
if all(item in ['comp', 'icx', 'sd'] for item in orders.values()) == False:
raise ValueError("orders must be in comp, icx, sd")
M = all(elem in groups.keys() for elem in X.columns)
if M == False:
raise ValueError("some variables must be used in groups and in X")
thresholds = np.sort(np.unique(y))
nThr = len(thresholds)
if nThr == 1:
raise ValueError("y must contain more than 1 distinct value")
Xp = X.copy()
Xp = prepareData(Xp, groups, orders)
nVar = Xp.shape[1]
oldNames = Xp.columns
Xp['y'] = y
Xp['ind'] = np.arange(len(y))
tt = list(oldNames)
X_grouped = Xp.groupby(tt).agg({'y': list, 'ind': list})
X_grouped = X_grouped.sort_values(by=list(oldNames)[::-1])
X_grouped = X_grouped.reset_index()
cpY = X_grouped["y"]
indices = X_grouped["ind"]
Xp = X_grouped[tt]
lenlist = np.vectorize(len)
weights = lenlist(indices)
N = Xp.shape[0]
if nVar == 1:
constr = None
rep_time = [len(x) for x in indices]
flat_indices = [item for sublist in cpY for item in sublist]
posY = np.repeat(np.arange(len(indices)),
rep_time)[np.argsort(flat_indices, kind="mergesort")]
cdf_vec = isocdf_seq(np.array(weights), np.ones(y.shape[0]),
np.sort(y), posY, thresholds)
cdf1 = np.reshape(cdf_vec, (N, nThr), order="F")
else:
constr = comp_ord(Xp)
cdf = np.zeros((N, nThr - 1))
A = tr_reduc(constr[0], N)
nConstr = A.shape[1]
l = np.zeros(nConstr)
A = sparse.csc_matrix((np.repeat([1, -1], nConstr),
(np.tile(np.arange(nConstr), 2), A.flatten())),
shape=(nConstr, N))
P = sparse.csc_matrix((weights, (np.arange(N), np.arange(N))))
i = 0
I = nThr - 1
#conv = np.full(I, False, dtype=bool)
q = -weights * np.array(
cpY.apply(lambda x: np.mean(np.array(x) <= thresholds[i])))
m = osqp.OSQP()
m.setup(P=P,
q=q,
A=A,
l=l,
verbose=verbose,
max_iter=max_iter,
eps_rel=eps_rel,
eps_abs=eps_abs)
sol = m.solve()
pmax = np.where(sol.x > 0, sol.x, 0)
cdf[:, 0] = np.where(pmax < 1, pmax, 1)
if I > 1:
for i in tqdm(range(1, I)):
m.warm_start(x=cdf[:, i - 1])
q = -weights * np.array(
cpY.apply(lambda x: np.mean(np.array(x) <= thresholds[i])))
m.update(q=q)
sol = m.solve()
pmax = np.where(sol.x > 0, sol.x, 0)
cdf[:, i] = np.where(pmax < 1, pmax, 1)
if nVar > 1:
cdf = pavaCorrect(cdf)
cdf1 = np.ones((N, nThr))
cdf1[:, :-1] = cdf
idr_object = idrobject(ecdf=cdf1,
thresholds=thresholds,
indices=indices,
X=Xp,
y=cpY,
groups=groups,
orders=orders,
constraints=constr)
return (idr_object)
def _quadprog(cls, Q, y, C, solver='osqp'):
"""solve min_x x^TPx + q^Tx, s.t. Gx<=h, Ax=b
Parameters
----------
Q : [type]
[description]
y : [type]
[description]
C : [type]
[description]
solver : str, optional
[description], by default 'osqp'
Returns
-------
[type]
[description]
"""
# dual
nl = y.shape[0]
q = -1 * np.ones((nl, 1))
if solver == 'cvxopt':
G = np.zeros((2 * nl, nl))
G[:nl, :] = -1 * np.eye(nl)
G[nl:, :] = np.eye(nl)
h = np.zeros((2 * nl, 1))
h[nl:, :] = C / nl
# convert numpy matrix to cvxopt matrix
P = matrix(Q)
q = matrix(q)
G = matrix(G)
h = matrix(h)
A = matrix(y.reshape(1, -1).astype('float64'))
b = matrix(np.zeros(1).astype('float64'))
solvers.options['show_progress'] = False
sol = solvers.qp(P, q, G, h, A, b)
alpha = np.array(sol['x']).reshape(nl)
elif solver == 'osqp':
warnings.simplefilter('ignore', sparse.SparseEfficiencyWarning)
P = sparse.csc_matrix((nl, nl))
P[:nl, :nl] = Q[:nl, :nl]
G = sparse.vstack([sparse.eye(nl), y.reshape(1, -1)]).tocsc()
l_ = np.zeros((nl + 1, 1))
u = np.zeros(l_.shape)
u[:nl, 0] = C
prob = osqp.OSQP()
prob.setup(P, q, G, l_, u, verbose=False)
res = prob.solve()
alpha = res.x
else:
raise ValueError('Invalid QP solver')
return alpha
def mpc_solver(p_0, max_v, min_v, max_a, min_a, max_j, min_j, Target_p, step_n):
v_0 = 0
a_0 = 0
j_0 = 0
K = 20
dt = 0.2
w1 = 10
w2 = 1
w3 = 1
w4 = 5
w5 = 1e8
count = 0
log_p = []
log_v = []
log_a = []
log_j = []
conc_with_identity = lambda x, y: np.concatenate([x, y * np.ones([K, K])], axis=1)
for i in range(step_n-1):
Tp, Tv, Ta, Bp, Bv, Ba = getPredictionMatrix(K, dt, p_0, v_0, a_0)
Target_p_K = np.asarray(Target_p[count:count + K]).reshape(1, -1)
H = scipy.linalg.block_diag(w4 * np.ones([K, K]) + w1 * (np.matmul(Tp.transpose(), Tp)),
w5 * np.ones([K, K]))
F = np.concatenate([2 * w1 * (np.matmul(Bp.transpose(), Tp) - np.matmul(Target_p_K, Tp)),
np.zeros([1, K])], axis=1)
A = np.concatenate([conc_with_identity(Tv, 0),
conc_with_identity(-Tv, -1),
conc_with_identity(Ta, 0),
conc_with_identity(-Ta, -1),
conc_with_identity(np.ones([K, K]), 0),
conc_with_identity(-np.ones([K, K]), -1),
conc_with_identity(np.zeros_like(Ta), -1)], axis=0)
b = np.concatenate([np.ones([K, 1]) * max_v - Bv,
-np.ones([K, 1]) * min_v + Bv,
np.ones([K, 1]) * max_a - Ba,
-np.ones([K, 1]) * min_a + Ba,
np.ones([K, 1]) * max_j,
-np.ones([K, 1]) * min_j,
np.zeros([K, 1])], axis=0)
P = sparse.csc_matrix(H)
q = np.array(F.T)
A_ = sparse.csc_matrix(A)
l = np.array(-np.inf * np.ones_like(b))
# l = None
u = np.array(b)
# Create an OSQP object
prob = osqp.OSQP()
# Setup workspace and change alpha parameter
prob.setup(P, q, A_, l, u)
# Solve problem
res = prob.solve()
j = res.x[0]
p_0 = p_0 + v_0 * dt + 0.5 * a_0 * dt ** 2 + 1 / 6 * j * dt ** 3
v_0 = v_0 + a_0 * dt + 0.5 * j * dt ** 2
a_0 = a_0 + j * dt
j_0 = j
log_p.append(p_0)
log_v.append(v_0)
log_a.append(a_0)
log_j.append(j_0)
count += 1
return log_p, log_v, log_a, log_j
def setup_OSQP(self,final_point):
[nx, nu] = self._osqp_Bd.shape
# Constraints
umin = np.ones(nu)*0.3-self.u_hover
umax = np.ones(nu)*0.9-self.u_hover
xmin = np.array([0.28,-2.]) #TODO: Tune/consider when testing on hw
xmax = np.array([ 5.0, 5.0])
# Sizes
self.ns = nx # p_x, p_y, p_z, v_x, v_y, v_z
self.nu = nu # f_x, f_y, f_z
# Objective function
Q = sparse.diags([5., .5])
QN = Q
R = 4.0*sparse.eye(nu)
# Initial and reference states
x0 = np.array([1.0,0.0])
xr = np.array([final_point[2], 0.0])
# Prediction horizon
N = int(self.rate*self.mpc_horizon)
self._osqp_N = N
# Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
# - quadratic objective
P = sparse.block_diag([sparse.kron(sparse.eye(N), Q), QN,
sparse.kron(sparse.eye(N), R)]).tocsc()
# - linear objective
q = np.hstack([np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr),
np.zeros(N*nu)])
# - linear dynamics
Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(nx)) + sparse.kron(sparse.eye(N+1, k=-1), self._osqp_Ad)
Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), self._osqp_Bd)
Aeq = sparse.hstack([Ax, Bu])
leq = np.hstack([-x0, np.zeros(N*nx)])
ueq = leq
# - input and state constraints
nf = 1 # do not add the first nf points to the constraints
xminf = -np.ones(nx)*np.inf
xmaxf = +np.ones(nx)*np.inf
Aineq = sparse.eye((N+1)*nx + N*nu)
#lineq = np.hstack([np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin)])
#uineq = np.hstack([np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax)])
lineq = np.hstack([np.kron(np.ones(nf), xminf), np.kron(np.ones(N+1-nf), xmin), np.kron(np.ones(N), umin)])
uineq = np.hstack([np.kron(np.ones(nf), xmaxf), np.kron(np.ones(N+1-nf), xmax), np.kron(np.ones(N), umax)])
# - OSQP constraints
A = sparse.vstack([Aeq, Aineq]).tocsc()
self._osqp_l = np.hstack([leq, lineq])
self._osqp_u = np.hstack([ueq, uineq])
Nx = (N+1)*nx
Nu = N*nu
if self.soft:
Pdelta = sparse.kron(sparse.eye(N+1), self.D)
P = sparse.block_diag([P,Pdelta])
qdelta = np.zeros(Nx)
q = np.hstack([q,qdelta])
Adelta = sparse.csc_matrix(np.vstack([np.zeros((Nx,Nx)),np.eye(Nx),np.zeros((Nu,Nx))]))
A = sparse.hstack([A, Adelta])
self.qp_size = P.shape[0]
# Create an OSQP object
self.prob = osqp.OSQP()
# Setup workspace
self.prob.setup(P, q, A, self._osqp_l, self._osqp_u, warm_start=True, verbose=False)
self.first = True
def solve_loop(qp_matrices, solver='osqp', osqp_settings=None):
"""
Solve lasso optimization loop for all lambdas
"""
# Shorter name for qp_matrices
qp = qp_matrices
# Extract features
n = qp.n
print('n = %d and solver %s' % (n, solver))
# Get number of problems to solve
n_prob = qp.q.shape[1]
# Initialize time vector
time = np.zeros(n_prob)
# Initialize number of iterations vector
niter = np.zeros(n_prob)
if solver == 'osqp':
# Setup OSQP
m = osqp.OSQP()
m.setup(qp.P, qp.q[:, 0], qp.A, qp.l, qp.u, **osqp_settings)
for i in range(n_prob):
q = qp.q[:, i]
# Update linear cost
m.update(q=q)
# Solve
results = m.solve()
x = results.x
y = results.y
status = results.info.status_val
niter[i] = results.info.iter
time[i] = results.info.run_time
# Check if status correct
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# DEBUG
# solve with gurobi
# prob = mpbpy.QuadprogProblem(qp.P, q, Aosqp, losqp, uosqp)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# print('Norm difference OSQP-GUROBI %.3e' %
# np.linalg.norm(x - res.x))
# import ipdb; ipdb.set_trace()
elif solver == 'osqp_coldstart':
# Setup OSQP
m = osqp.OSQP()
m.setup(qp.P, qp.q[:, 0], qp.A, qp.l, qp.u, **osqp_settings)
for i in range(n_prob):
q = qp.q[:, i]
# Update linear cost
m.update(q=q)
# Solve
results = m.solve()
x = results.x
y = results.y
status = results.info.status_val
niter[i] = results.info.iter
time[i] = results.info.run_time
# Check if status correct
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# DEBUG
# solve with gurobi
# prob = mpbpy.QuadprogProblem(qp.P, q, qp.A, qp.l, qp.u)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# print('Norm difference OSQP-GUROBI %.3e' %
# np.linalg.norm(x - res.x))
# import ipdb; ipdb.set_trace()
# DEBUG print iterations per value of lambda
# lambda_vals = np.logspace(-2, 2, 101)[::-1]
#
# import matplotlib.pylab as plt
# plt.figure()
# ax = plt.gca()
# plt.plot(lambda_vals, niter)
# ax.set_xlabel(r'$\lambda$')
# ax.set_ylabel(r'iter')
# plt.show(block=False)
# import ipdb; ipdb.set_trace()
elif solver == 'osqp_no_caching':
for i in range(n_prob):
# Setup OSQP
m = osqp.OSQP()
m.setup(qp.P, qp.q[:, i], qp.A, qp.l, qp.u, **osqp_settings)
# Solve
results = m.solve()
x = results.x
y = results.y
status = results.info.status_val
niter[i] = results.info.iter
time[i] = results.info.run_time
# Check if status correct
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# DEBUG
# solve with gurobi
# prob = mpbpy.QuadprogProblem(qp.P, q, Aosqp, losqp, uosqp)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# print('Norm difference OSQP-GUROBI %.3e' %
# np.linalg.norm(x - res.x))
# import ipdb; ipdb.set_trace()
elif solver == 'qpoases':
n_dim = qp.P.shape[0] # Number of variables
m_dim = qp.A.shape[0] # Number of constraints without bounds
# Initialize qpoases and set options
qpoases_m = qpoases.PyQProblem(n_dim, m_dim)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qpoases_m.setOptions(options)
# Setup matrix P and A
P = np.ascontiguousarray(qp.P.todense())
A = np.ascontiguousarray(qp.A.todense())
for i in range(n_prob):
# Get linera cost as contiguous array
q = np.ascontiguousarray(qp.q[:, i])
# Reset cpu time
qpoases_cpu_time = np.array([10.])
# Reset number of of working set recalculations
nWSR = np.array([1000])
if i == 0:
# First iteration
res_qpoases = qpoases_m.init(P, q, A,
np.ascontiguousarray(qp.lx),
np.ascontiguousarray(qp.ux),
np.ascontiguousarray(qp.l),
np.ascontiguousarray(qp.u), nWSR,
qpoases_cpu_time)
else:
# Solve new hot started problem
res_qpoases = qpoases_m.hotstart(q,
np.ascontiguousarray(qp.lx),
np.ascontiguousarray(qp.ux),
np.ascontiguousarray(qp.l),
np.ascontiguousarray(qp.u),
nWSR, qpoases_cpu_time)
# # DEBUG Solve with gurobi
# qpoases solution
# sol_qpoases = np.zeros(n + k)
# qpoases_m.getPrimalSolution(sol_qpoases)
# import mathprogbasepy as mpbpy
# Agrb = spa.vstack((qp.A,
# spa.hstack((spa.eye(n), spa.csc_matrix((n, k)))
# ))).tocsc()
# lgrb = np.append(qp.l, qp.lx)
# ugrb = np.append(qp.u, qp.ux)
# prob = mpbpy.QuadprogProblem(spa.csc_matrix(qp.P), q,
# Agrb, lgrb, ugrb)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=True)
# print("Norm difference x qpoases - GUROBI = %.4f" %
# np.linalg.norm(sol_qpoases - res.x))
# print("Norm difference objval qpoases - GUROBI = %.4f" %
# abs(qpoases_m.getObjVal() - res.obj_val))
# import ipdb; ipdb.set_trace()
# if res_qpoases != 0:
# raise ValueError('qpoases did not solve the problem!')
# Save time
time[i] = qpoases_cpu_time[0]
# Save number of iterations
niter[i] = nWSR[0]
elif solver == 'gurobi':
for i in range(n_prob):
# Get linera cost as contiguous array
q = qp.q[:, i]
# Solve with gurobi
prob = mpbpy.QuadprogProblem(qp.P, q, qp.A, qp.l, qp.u)
res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# Save time
time[i] = res.cputime
# Save number of iterations
niter[i] = res.total_iter
elif solver == 'mosek':
for i in range(n_prob):
# Get linera cost as contiguous array
q = qp.q[:, i]
# Solve with mosek
prob = mpbpy.QuadprogProblem(qp.P, q, qp.A, qp.l, qp.u)
res = prob.solve(solver=mpbpy.MOSEK, verbose=False)
# Save time
time[i] = res.cputime
# Save number of iterations
niter[i] = res.total_iter
elif solver == 'ecos':
n_var = qp_matrices.A_lasso.shape[1]
m_var = qp_matrices.A_lasso.shape[0]
for i in range(n_prob):
if n_var <= 60: # (problem becomes too big otherwise):
# Model with CVXPY
# minimize y' * y + lambda * 1' * t
# subject to y = Ax - b
# -t <= x <= t
lambda_i = qp_matrices.lambdas[i]
x = cvxpy.Variable(n_var)
y = cvxpy.Variable(m_var)
t = cvxpy.Variable(n_var)
objective = cvxpy.Minimize(
cvxpy.quad_form(y, spa.eye(m_var)) +
lambda_i * np.ones(n_var) * t)
constraints = [
y == qp_matrices.A_lasso * x - qp_matrices.b_lasso,
-t <= x, x <= t
]
problem = cvxpy.Problem(objective, constraints)
problem.solve(solver=cvxpy.ECOS, verbose=False)
# DEBUG: Solve with MOSEK
q = qp.q[:, i]
# Solve with mosek
# prob = mpbpy.QuadprogProblem(qp.P, q, qp.A, qp.l, qp.u)
# res = prob.solve(solver=mpbpy.MOSEK, verbose=False)
# x_mosek = res.x[:n_var]
# import ipdb; ipdb.set_trace()
# Obtain time and number of iterations
time[i] = problem.solver_stats.setup_time + \
problem.solver_stats.solve_time
niter[i] = problem.solver_stats.num_iters
else:
time[i] = 0
niter[i] = 0
else:
raise ValueError('Solver not understood')
# Return statistics
return utils.Statistics(time), utils.Statistics(niter)
def __init__(self):
# output_topic = rospy.get_param('~output_topic')
# subscribers and publishers
trigger_topic = rospy.get_param('~trigger_topic')
self.mpc_trigger_sub = rospy.Subscriber(trigger_topic, Empty,
self.osqp_trigger)
# model_yaml = rospy.get_param('~model_yaml')
# Prediction horizon
# N = 10
N = rospy.get_param('~N')
self.N = N
dt = rospy.get_param('~dt')
# dt = 0.25; # Assuming each increment is 0.25 seconds long
Ad_yaml = rospy.get_param('~Ad')
Ad_data = Ad_yaml['data']
print(Ad_data)
Ad = Ad_data
for i in range(len(Ad_data)):
# print("Ad_data[{}] = {}").format(i, Ad_data[i])
if isinstance(Ad_data[i], str):
Ad[i] = eval(Ad_data[i])
# print("Ad[{}] = {}").format(i, Ad[i])
test_mat = self.yaml_matrix2csc(Ad, Ad_yaml['rows'], Ad_yaml['rows'])
print(test_mat)
# Discrete time model of a 1D quadcopter
self.Ad = sparse.csc_matrix([[1.0, dt, dt * dt / 2], [0.0, 1.0, dt],
[0.0, 0.0, 1.0]])
self.Bd = sparse.csc_matrix([[0.0], [0.0], [1.0]])
[nx, nu] = self.Bd.shape
self.nx = nx
self.nu = nu
# Objective function
Q = sparse.diags([1.0, 1.0, 0.0])
# Q = 1.0*sparse.eye(nx)
QN = Q # Q at horizon N
R = 0.1 * sparse.eye(self.nu)
# Constraints
u0 = 0.0
umin = np.array([-1.0]) - u0
umax = np.array([1.0]) - u0
xmin = np.array([-np.inf, -np.inf, -np.inf])
xmax = np.array([np.inf, np.inf, np.inf])
# Initial and reference states
self.x0 = np.zeros(3)
xr = np.array([1.0, 0.0, 0.0])
# Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
# - quadratic objective
P = sparse.block_diag(
[sparse.kron(sparse.eye(N), Q), QN,
sparse.kron(sparse.eye(N), R)]).tocsc()
# - linear objective
# Stack all
q = np.hstack([
np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr),
np.zeros(N * self.nu)
])
# - linear dynamics
Ax = sparse.kron(sparse.eye(N + 1), -sparse.eye(nx)) + sparse.kron(
sparse.eye(N + 1, k=-1), self.Ad)
Bu = sparse.kron(
sparse.vstack([sparse.csc_matrix((1, N)),
sparse.eye(N)]), self.Bd)
Aeq = sparse.hstack([Ax, Bu])
leq = np.hstack([-self.x0, np.zeros(N * nx)])
ueq = leq
# - input and state constraints
Aineq = sparse.eye((N + 1) * nx + N * self.nu)
lineq = np.hstack(
[np.kron(np.ones(N + 1), xmin),
np.kron(np.ones(N), umin)])
uineq = np.hstack(
[np.kron(np.ones(N + 1), xmax),
np.kron(np.ones(N), umax)])
# - OSQP constraints
A = sparse.vstack([Aeq, Aineq]).tocsc()
self.l = np.hstack([leq, lineq])
self.u = np.hstack([ueq, uineq])
# Create an OSQP object
self.osqp_solver = osqp.OSQP()
# Setup workspace
self.osqp_solver.setup(P, q, A, self.l, self.u, warm_start=True)
while not rospy.is_shutdown():
rospy.spin()
def test_issue14(self):
m = osqp.OSQP()
m.setup(self.P, self.q, self.A, self.l, self.u,
linsys_solver="mkl pardiso")
m.solve()
def forward_pass(self):
x = np.copy(self.initial_state)
feasible = False
trust_region_scale = 1
while not feasible:
feasible = True
current_J = 0
x_new_trajectories = np.zeros(
(self.system.state_size, self.horizon + 1))
u_new_trajectories = np.zeros(
(self.system.control_size, self.horizon))
x = np.copy(self.initial_state)
x_new_trajectories[:, 0] = np.copy(x)
for i in range(self.horizon):
delta_x = x - self.x_trajectories[:, i]
x_new_trajectories[:, i] = np.copy(x)
Q_ux = self.Q_UX[:, :, i]
Q_u = self.Q_U[:, i]
P = sparse.csr_matrix(self.Q_UU[:, :, i])
q = (Q_ux.dot(delta_x) + Q_u)
'''lb = -self.system.control_bound - self.u_trajectories[:, i]
ub = self.system.control_bound - self.u_trajectories[:, i]
lb *= trust_region_scale
ub *= trust_region_scale'''
#constraint_A = sparse.csr_matrix(np.identity(self.system.control_size))
#initialize contraint matrix and bound
constraint_A = np.zeros(
(self.system.control_size + len(self.constraints),
self.system.control_size))
lb = np.zeros(self.system.control_size + len(self.constraints))
ub = np.zeros(self.system.control_size + len(self.constraints))
#control limit contraint
constraint_A[0:self.system.control_size,
0:self.system.control_size] = np.identity(
self.system.control_size)
lb[0:self.system.control_size] = 0 - self.u_trajectories[:, i]
ub[0:self.system.
control_size] = self.system.control_bound - self.u_trajectories[:,
i]
lb *= trust_region_scale
ub *= trust_region_scale
#formulate linearized state constraints
f_x, f_u = self.system.transition_J(x, self.u_trajectories[:,
i])
constraint_index = self.system.control_size
for constraint in self.constraints:
if i <= self.horizon - 2: #current action might cause state constraint violation
x_temp = self.system.transition(
x, self.u_trajectories[:, i])
D = constraint.evaluate_constraint(x_temp)
#print("constraint eval", D, i, x)
C = constraint.evaluate_constraint_J(x_temp)
#print(C.shape, f_u.shape)
C = C.dot(f_u)
constraint_A[constraint_index, :] = np.copy(C)
lb[constraint_index] = -np.inf #no lower bound
ub[constraint_index] = -D
constraint_index += 1
constraint_A = sparse.csr_matrix(constraint_A)
prob = osqp.OSQP()
prob.setup(P,
q,
constraint_A,
lb,
ub,
alpha=1.0,
verbose=False)
res = prob.solve()
if res.info.status != 'solved':
feasible = False
#print("infeasible, reduce trust region")
trust_region_scale *= 0.5
break
delta_u = res.x[0:self.system.control_size]
u = delta_u + self.u_trajectories[:, i]
u_new_trajectories[:, i] = np.copy(u)
# print(u)
current_J += self.system.calculate_cost(x, u)
x = self.system.transition(x, u)
x_new_trajectories[:, self.horizon] = np.copy(x)
current_J += self.system.calculate_final_cost(x)
if current_J > self.best_J + 100:
feasible = False
trust_region_scale *= 0.5
else:
self.best_J = current_J
if feasible == True:
self.x_trajectories = np.copy(x_new_trajectories)
self.u_trajectories = np.copy(u_new_trajectories)
print("total cost", current_J)
def compute_min_derivative_multispline(order, min_derivative_order,
continuity_order, traj_waypoints):
num_segments = len(traj_waypoints) - 1
if num_segments < 2:
return None
cw = order + 1 # constraint width
x_dim = cw * num_segments # num columns in the constraint matrix
multi_x_dim = x_dim * traj_waypoints[0].ndim
Aeq = np.zeros((0, 0)) # equality constraints A matrix
Aieq = np.zeros((0, 0)) # inequality constraints A matrix
beq = np.zeros((0, 1))
bieq = np.zeros((0, 1))
H = np.zeros((0, 0))
for dim in range(traj_waypoints[0].ndim):
pins = [wp.spline_pins[dim] for wp in traj_waypoints]
dAeq, dbeq, dAieq, dbieq, dH = compute_spline_matrices(
order, min_derivative_order, continuity_order, pins)
Aeq = linalg.block_diag(Aeq, dAeq)
Aieq = linalg.block_diag(Aieq, dAieq)
H = linalg.block_diag(H, dH)
beq = np.vstack((beq, dbeq))
bieq = np.vstack((bieq, dbieq))
# build directional constraints (constraints between spline dimensions)
for seg in range(num_segments):
wp = traj_waypoints[seg]
for sdc in wp.soft_directional_constraints:
con_order = sdc[0]
dvec = sdc[1]
radius = sdc[2]
dspace = np.zeros((wp.ndim, wp.ndim))
dspace[0, :] = np.array(dvec)
nspace = linalg.null_space(dspace)
nullspace_vecs = list(nspace.transpose())
# Positive dot product enforces correct direction of motion
new_constraint = np.zeros((1, multi_x_dim))
for d in range(wp.ndim):
scalar = dvec[d]
tvec = _calc_tvec(0, order, con_order)
vec = scalar * tvec
new_constraint[0, x_dim * d + seg * cw:x_dim * d +
(seg + 1) * cw] = vec
Aieq = np.vstack((Aieq, -1 * new_constraint))
bieq = np.vstack((bieq, 0))
# Limit motion in null space:
for v in nullspace_vecs:
new_constraint = np.zeros((1, multi_x_dim))
for d in range(wp.ndim):
scalar = v[d]
tvec = _calc_tvec(0, order, con_order)
vec = scalar * tvec
new_constraint[0, x_dim * d + seg * cw:x_dim * d +
(seg + 1) * cw] = vec
Aieq = np.vstack((Aieq, new_constraint))
Aieq = np.vstack((Aieq, -1 * new_constraint))
bieq = np.vstack((bieq, radius))
bieq = np.vstack((bieq, -radius))
# Convert equality constraints to inequality constraints:
Aeq_ieq = np.vstack((Aeq, -1 * Aeq))
beq_ieq = np.vstack((beq, -1 * beq))
Aieq = np.vstack((Aieq, Aeq_ieq))
bieq = np.vstack((bieq, beq_ieq))
# Solve the QP!
m = osqp.OSQP()
try:
m.setup(P=sparse.csc_matrix(H),
q=None,
l=None,
A=sparse.csc_matrix(Aieq),
u=bieq,
verbose=False)
except ValueError as ve:
print(ve.message)
print("Could not setup QP!")
return None
results = m.solve()
x = results.x
splines = []
xwidth = len(x) / traj_waypoints[0].ndim
for dim in range(traj_waypoints[0].ndim):
dx = x[dim * xwidth:dim * xwidth + xwidth]
coefficients = np.fliplr(
np.array(dx).reshape((num_segments, order + 1)))
ts = [wp.time for wp in traj_waypoints]
spline = OptimalSpline(coefficients.transpose(), ts)
splines.append(spline)
return splines
def mpc_(Ad_mat, Bd_mat, gd_mat, x_vec, Xr, Q, QN, R, N, lb_x, ub_x, lb_y,
ub_y, umin, umax):
# ========== Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1)) ==========
nx = Ad_mat.shape[0]
nu = Bd_mat.shape[1]
Ad = sparse.csc_matrix(Ad_mat)
Bd = sparse.csc_matrix(Bd_mat)
gd = np.squeeze(gd_mat, axis=1) # from (N,1) to (N,)
# ----- quadratic objective -----
P = sparse.block_diag(
[sparse.kron(sparse.eye(N), Q), QN,
sparse.kron(sparse.eye(N), R)]).tocsc()
# ----- linear objective -----
# xr_vec = np.squeeze(xr, axis=1)
# q = np.hstack([np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr),
# np.zeros(N*nu)])
# q = np.hstack([np.kron(np.ones(N), -Q.dot(xr_vec)), -QN.dot(xr_vec),
# np.zeros(N*nu)])
q = -Q.dot(Xr[:, 0]) # index 0
for ii in range(N - 1):
q = np.hstack([q, -Q.dot(Xr[:, ii + 1])]) # index 1 ~ N-1
q = np.hstack([q, -QN.dot(Xr[:, -1]), np.zeros(N * nu)]) # index N
# ----- linear dynamics -----
Ax = sparse.kron(sparse.eye(N + 1), -sparse.eye(nx)) + sparse.kron(
sparse.eye(N + 1, k=-1), Ad)
Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)),
sparse.eye(N)]), Bd)
Aeq = sparse.hstack([Ax, Bu])
# leq = np.hstack([-x0, np.zeros(N*nx)])
# leq = np.hstack([-x0, np.kron(np.ones(N), -gd)])
leq = np.hstack([-x_vec, np.kron(np.ones(N), -gd)])
ueq = leq
# ----- input and state constraints -----
Aineq = sparse.eye((N + 1) * nx + N * nu)
lineq = []
uineq = []
# lineq = np.hstack([np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin)])
# uineq = np.hstack([np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax)])
for i in range(len(lb_x)):
xmin = [lb_x[i], lb_y[i], -10, -np.pi]
xmax = [ub_x[i], ub_y[i], 10, np.pi]
lineq = np.hstack([lineq, xmin])
uineq = np.hstack([uineq, xmax])
lineq = np.hstack([lineq, np.kron(np.ones(N), umin)])
uineq = np.hstack([uineq, np.kron(np.ones(N), umax)])
# ----- OSQP constraints -----
A = sparse.vstack([Aeq, Aineq]).tocsc()
lb = np.hstack([leq, lineq])
ub = np.hstack([ueq, uineq])
# ==========Create an OSQP object and Setup workspace ==========
prob = osqp.OSQP()
prob.setup(P, q, A, lb, ub, verbose=False,
warm_start=True) # verbose: print output.
# Solve
res = prob.solve()
return res
def mpc__(Ad_list, Bd_list, gd_list, x_vec, Xr, Q, QN, R, N, xmin, xmax, umin,
umax):
"""
Initialize Nonlinear Dynamics with Shooting Method
"""
# ========== Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1)) ==========
nx = Ad_list[0].shape[0]
nu = Bd_list[0].shape[1]
# ----- quadratic objective -----
P = sparse.block_diag(
[sparse.kron(sparse.eye(N), Q), QN,
sparse.kron(sparse.eye(N), R)]).tocsc()
# ----- linear objective -----
# xr_vec = np.squeeze(xr, axis=1)
# q = np.hstack([np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr),
# np.zeros(N*nu)])
# q = np.hstack([np.kron(np.ones(N), -Q.dot(xr_vec)), -QN.dot(xr_vec),
# np.zeros(N*nu)])
q = -Q.dot(Xr[:, 0]) # index 0
for ii in range(N - 1):
q = np.hstack([q, -Q.dot(Xr[:, ii + 1])]) # index 1 ~ N-1
q = np.hstack([q, -QN.dot(Xr[:, -1]), np.zeros(N * nu)]) # index N
# ----- linear dynamics -----
Ax_Ad = sparse.csc_matrix(Ad_list[0])
Ax_diag = sparse.kron(sparse.eye(N + 1), -sparse.eye(nx))
Bu_Bd = sparse.csc_matrix(Bd_list[0])
for i in range(N - 1):
Ad = sparse.csc_matrix(Ad_list[i + 1])
Bd = sparse.csc_matrix(Bd_list[i + 1])
Ax_Ad = sparse.block_diag([Ax_Ad, Ad])
Bu_Bd = sparse.block_diag([Bu_Bd, Bd])
Ax_Ad_top = sparse.kron(np.ones(N + 1), np.zeros((nx, nx)))
Ax_Ad_side = sparse.kron(np.ones((N, 1)), np.zeros((nx, nx)))
Ax = Ax_diag + sparse.vstack(
[Ax_Ad_top, sparse.hstack([Ax_Ad, Ax_Ad_side])])
Bu_Bd_top = sparse.kron(np.ones(N), np.zeros((nx, nu)))
Bu = sparse.vstack([Bu_Bd_top, Bu_Bd])
Aeq = sparse.hstack([Ax, Bu])
leq = -x_vec # later ueq == leq
for i in range(N):
gd = np.squeeze(gd_list[i], axis=1) # from (N,1) to (N,)
leq = np.hstack([leq, -gd])
ueq = leq
# Original Code
# Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(nx)) + sparse.kron(sparse.eye(N+1, k=-1), Ad_list[0])
# Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), Bd_list[0])
# Aeq = sparse.hstack([Ax, Bu])
# gd = np.squeeze(gd_list[0], axis=1) # from (N,1) to (N,)
# leq = np.hstack([-x_vec, np.kron(np.ones(N), -gd)])
# ueq = leq
# ----- input and state constraints -----
Aineq = sparse.eye((N + 1) * nx + N * nu)
lineq = np.hstack(
[np.kron(np.ones(N + 1), xmin),
np.kron(np.ones(N), umin)])
uineq = np.hstack(
[np.kron(np.ones(N + 1), xmax),
np.kron(np.ones(N), umax)])
# lineq = []
# uineq = []
# for i in range(len(lb_x)):
# xmin = [lb_x[i], lb_y[i], -10, -np.pi]
# xmax = [ub_x[i], ub_y[i], 10, np.pi]
# lineq = np.hstack([lineq, xmin])
# uineq = np.hstack([uineq, xmax])
# lineq = np.hstack([lineq, np.kron(np.ones(N), umin)])
# uineq = np.hstack([uineq, np.kron(np.ones(N), umax)])
# ----- OSQP constraints -----
A = sparse.vstack([Aeq, Aineq]).tocsc()
lb = np.hstack([leq, lineq])
ub = np.hstack([ueq, uineq])
# ==========Create an OSQP object and Setup workspace ==========
prob = osqp.OSQP()
prob.setup(P, q, A, lb, ub, verbose=False,
warm_start=True) # verbose: print output.
# Solve
res = prob.solve()
return res
def mpc(A, B, C, D, F0, G0, xk, ek, uk1, yrk1, xmax, xmin, umax, umin, dumax, dumin, ymax, ymin, yrmax, yrmin, dyrmax, dyrmin, qx, qe, qu, qyr, qxf, qef, zmax, zmin, Cz, Dz, Gz, N= 10, dT = 250e-3):
# =========================================================================================
# Calculos
# Calcular cantidad de elementos.
[nx, nu] = B.shape
[ny, nx] = C.shape
[nz, nx] = Cz.shape
# -------------------------------------------------------------------------
#Calcular las matrices del estado aumentado
A_ = sparse.vstack([
sparse.hstack([A, sparse.csc_matrix((nx, ny))]),
sparse.hstack([dT * C, sparse.eye(ny)])
], format='csc')
B_ = sparse.vstack([
sparse.hstack([B, sparse.csc_matrix((nx, ny))]),
sparse.hstack([dT * D, -1 * dT * sparse.eye(ny)])
], format='csc')
#Condiciones iniciales del estado aumentado
xk_ = np.hstack([xk, ek])
#Calcular tamaño del estado aumentado
[nx_, nu_] = B_.shape
#Calcular las matrices de las variables algebraicas considerando el estado aumentado.
Cz_ = sparse.hstack([Cz, sparse.csc_matrix((nz, ny))], format = 'csc')
Dz_ = sparse.hstack([Dz, sparse.csc_matrix((nz, ny))], format = 'csc')
# -------------------------------------------------------------------------
#Generar las matrices de pesos.
Q = sparse.diags(np.hstack([qx, qe]), format='csc')
R = sparse.diags(np.hstack([qu, qyr]), format='csc')
# Qf = computeQf(A_, B_, Q, R) # Calcular la ganancia de la condición final tal que cumpla la ecuación de Ricatti.
Qf = sparse.diags(np.hstack([qxf, qef]), format='csc') #Assigned directly.
# Input diferences
# dU = Kdu * u
# Kdu = sparse.kron(sparse.hstack([sparse.csc_matrix((N-1, 1)), sparse.eye(N-1)]), sparse.eye(nu_)) - sparse.kron(sparse.hstack([sparse.eye(N-1), sparse.csc_matrix((N-1, 1))]), sparse.eye(nu_))
# # Rdu = Kdu.T * R * Kdu
# Rdu = Kdu.transpose() * sparse.kron(sparse.eye(N-1), R ) * Kdu
# Plantear problema de optimización
# Problema de la forma : 0.5 xT P x + qT x
# - quadratic objective
qH = np.arange(1, N+1) / N
# P = sparse.block_diag([sparse.kron(sparse.diags(qH), Q), #Estados durante el horizonte de predicción
# Qf, # Estados al final del horizonte de predicción
# Rdu], format='csc') #Cambio en las entradas durante el horizonte de predicción
P = sparse.block_diag([sparse.kron(sparse.diags(qH), Q), #Estados durante el horizonte de predicción
Qf, # Estados al final del horizonte de predicción
sparse.kron(sparse.eye(N), R)], format='csc') #Entradas durante el horizonte de predicción
# - linear objective
q = np.zeros( (N+1)*nx_ + N*nu_)
#==============================================================================
#Restrictions
#------------------------------------------------------------------------------
#Equality restriction
#------------------------------------------------------------------------------
#linear dynamics.
# Aeq * [x, e, u, yr].T
Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(nx_)) + sparse.kron(sparse.eye(N+1, k=-1), A_)
Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), B_)
Aeq = sparse.hstack([Ax, Bu])
bk = - 1 * np.hstack([xk, ek, np.zeros(N*nx_)])
b0 = - 1 * np.hstack([np.zeros(nx_), np.kron(np.ones(N), np.hstack([F0, dT * G0]))])
b = b0 + bk
#Relajar un poco las restricciones de igualdad
leq = b - 0.05 * abs(b)
ueq = b + 0.05 * abs(b)
#------------------------------------------------------------------------------
#Inequality restriction
#------------------------------------------------------------------------------
# - Bounds!!!!!
xmin_ = np.hstack([xmin,np.kron(np.ones(ny), -np.inf)])
xmax_ = np.hstack([xmax,np.kron(np.ones(ny), np.inf)])
umin_ = np.hstack([umin, yrmin])
umax_ = np.hstack([umax, yrmax])
ABound = sparse.eye((N+1)*nx_ + N*nu_)
lBound= np.hstack([np.kron(np.ones(N+1), xmin_), np.kron(np.ones(N), umin_)])
uBound = np.hstack([np.kron(np.ones(N+1), xmax_), np.kron(np.ones(N), umax_)])
#Maximum rate of change
dumin_ = np.hstack([dumin, dyrmin])
dumax_ = np.hstack([dumax, dyrmax])
uk1_ = np.hstack([uk1, yrk1])
#Maximum rate of change
Au = sparse.hstack([sparse.csc_matrix((N*nu_, (N+1)*nx_)),
sparse.kron(sparse.eye(N), sparse.eye(nu_)) + sparse.kron(sparse.eye(N, k=-1), -1 * sparse.eye(nu_))])
ldu = np.hstack([dumin_ + uk1_, np.kron(np.ones(N-1), dumin_)])
udu = np.hstack([dumax_ + uk1_, np.kron(np.ones(N-1), dumax_)])
# Algebraic variables
Az = sparse.hstack([
sparse.kron(sparse.hstack([sparse.eye(N), sparse.csc_matrix((N, 1))]), Cz_),
sparse.kron(sparse.eye(N), Dz_)
])
lz = np.kron(np.ones(N), zmin) - np.kron(np.ones(N), Gz)
uz = np.kron(np.ones(N), zmax) - np.kron(np.ones(N), Gz)
#------------------------------------------------------------------------------
# - Solver constraints
#In general, the contraints have the form:
# lb <= Ax <= ub
Aconst = sparse.vstack([Aeq, ABound, Au, Az], format='csc')
lb = np.hstack([leq, lBound, ldu, lz])
ub = np.hstack([ueq, uBound, udu, uz])
#Condiciones iniciales para las variables de optimización [x, e, u] a lo largo de todo el horizonte de predicción.
_Xk_ = np.hstack ([np.kron(np.ones(N+1), xk_),np.kron(np.ones(N), uk1_)])
# Create an OSQP object
prob = osqp.OSQP()
# ------------------------------------
# Setup workspace
#eps_abs: Absolute tolerance
# eps_prim_inf: Primal infeasibility tolerance
# eps_dual_inf: Dual infeasibility tolerance
prob.setup(P, q, Aconst, lb, ub, verbose = False, polish = True, eps_abs = 1e-6, eps_prim_inf=1e-5, eps_dual_inf = 1e-03)
# ------------------------------------
#Start using initial conditions.
prob.warm_start(x = _Xk_)
#Solve problem
res = prob.solve()
# Check solver status
if res.info.status not in ['solved', 'solved inaccurate']:
# The inaccurate status define when the optimality, primal infeasibility or dual infeasibility conditions are satisfied with tolerances 10 times largerthan the ones set.
raise ValueError('OSQP did not solve the problem!')
# Get firts inputs of the augmented state.
_uk_ = res.x[-N*nu_:-(N-1)*nu_]
uk = _uk_[:nu]
yrk = _uk_[-ny:]
# output = res.x[:N*nx_]
# output = output.reshape((N, nx_))
# plt.plot(output[:, 28])
# plt.show()
z = (Az * res.x + np.kron(np.ones(N), Gz))[-N*nz:-(N-1)*nz]
return uk, yrk, z
def compute_min_derivative_spline(order, min_derivative_order,
continuity_order, waypoints):
num_segments = len(waypoints) - 1
if num_segments < 1:
return None
assert not any([wp.time is None for wp in waypoints])
waypoints.sort(key=lambda waypoint: waypoint.time)
durations = [0] * num_segments
for i in range(len(durations)):
durations[i] = waypoints[i + 1].time - waypoints[i].time
# Start with waypoint constraints: (Start of each segment, end of last segment.)
# x is vertical stack of the coefficient col vectors for each segment
cw = order + 1 # constraint width
x_dim = cw * (num_segments) # num columns in the constraint matrix
Aeq = np.zeros((0, x_dim)) # equality constraints A matrix
Aieq = np.zeros((0, x_dim)) # inequality constraints A matrix
beq = np.zeros((0, 1))
bieq = np.zeros((0, 1))
for seg, wp in enumerate(waypoints):
for con in wp.hard_constraints:
if seg == num_segments:
tvec = _calc_tvec(durations[seg - 1], order, con[0])
i = seg - 1
else:
tvec = _calc_tvec(0, order, con[0])
i = seg
constraint = np.zeros(x_dim) # hard constraint at waypoint
constraint[i * cw:(i + 1) * cw] = tvec
Aeq = np.vstack((Aeq, constraint))
beq = np.vstack((beq, con[1]))
for con in wp.soft_constraints: # voxel constraints around waypoint
if seg == num_segments:
tvec = _calc_tvec(durations[seg - 1], order, con[0])
i = seg - 1
else:
tvec = _calc_tvec(0, order, con[0])
i = seg
constraint_max = np.zeros(x_dim)
constraint_max[i * cw:(i + 1) * cw] = tvec
constraint_min = np.zeros(x_dim)
constraint_min[i * cw:(i + 1) * cw] = -1 * tvec
Aieq = np.vstack((Aieq, constraint_max, constraint_min))
bieq = np.vstack((bieq, con[1] + con[2], -(con[1] - con[2])))
# Continuity constraints: (tvec_a(t=end) = tvec_b(t=0))
for seg in range(0, num_segments - 1):
for r in range(0, continuity_order + 1):
constraint = np.zeros(x_dim) # hard constraint at waypoint
tvec_end = _calc_tvec(durations[seg], order, r)
tvec_start = _calc_tvec(0, order, r)
constraint[seg * cw:(seg + 1) * cw] = tvec_end
constraint[(seg + 1) * cw:(seg + 2) * cw] = -tvec_start
Aeq = np.vstack((Aeq, constraint))
beq = np.vstack((beq, [0]))
# Construct Hermitian matrix:
H = np.zeros((x_dim, x_dim))
for seg in range(0, num_segments):
Q = _compute_Q(order, min_derivative_order, 0, durations[seg])
H[cw * seg:cw * (seg + 1), cw * seg:cw * (seg + 1)] = Q
c = np.zeros((x_dim, 1))
# Convert equality constraints to inequality constraints:
Aeq_ieq = np.vstack((Aeq, -1 * Aeq))
beq_ieq = np.vstack((beq, -1 * beq))
Aieq = np.vstack((Aieq, Aeq_ieq))
bieq = np.vstack((bieq, beq_ieq))
# Solve the QP!
m = osqp.OSQP()
m.setup(P=sparse.csc_matrix(H),
q=None,
l=None,
A=sparse.csc_matrix(Aieq),
u=bieq,
verbose=False)
results = m.solve()
x = results.x
coefficients = np.fliplr(np.array(x).reshape((num_segments, order + 1)))
ts = [wp.time for wp in waypoints]
return OptimalSpline(coefficients.transpose(), ts)
def _init_problem(self):
"""
Initialize optimization problem for current time step.
"""
# Constraints
umin = self.input_constraints['umin']
umax = self.input_constraints['umax']
xmin = self.state_constraints['xmin']
xmax = self.state_constraints['xmax']
# LTV System Matrices
A = np.zeros((self.nx * (self.N + 1), self.nx * (self.N + 1)))
B = np.zeros((self.nx * (self.N + 1), self.nu * (self.N)))
# Reference vector for state and input variables
ur = np.zeros(self.nu*self.N)
xr = np.zeros(self.nx*(self.N+1))
# Offset for equality constraint (due to B * (u - ur))
uq = np.zeros(self.N * self.nx)
# Dynamic state constraints
xmin_dyn = np.kron(np.ones(self.N + 1), xmin)
xmax_dyn = np.kron(np.ones(self.N + 1), xmax)
# Dynamic input constraints
umax_dyn = np.kron(np.ones(self.N), umax)
# Get curvature predictions from previous control signals
kappa_pred = np.tan(np.array(self.current_control[3::] +
self.current_control[-1:])) / self.model.length
# Iterate over horizon
for n in range(self.N):
# Get information about current waypoint
current_waypoint = self.model.reference_path.get_waypoint(
self.model.wp_id + n)
next_waypoint = self.model.reference_path.get_waypoint(
self.model.wp_id + n + 1)
delta_s = next_waypoint - current_waypoint
kappa_ref = current_waypoint.kappa
v_ref = current_waypoint.v_ref
# Compute LTV matrices
f, A_lin, B_lin = self.model.linearize(v_ref, kappa_ref, delta_s)
A[(n+1) * self.nx: (n+2)*self.nx, n * self.nx:(n+1)*self.nx] = A_lin
B[(n+1) * self.nx: (n+2)*self.nx, n * self.nu:(n+1)*self.nu] = B_lin
# Set reference for input signal
ur[n*self.nu:(n+1)*self.nu] = np.array([v_ref, kappa_ref])
# Compute equality constraint offset (B*ur)
uq[n * self.nx:(n+1)*self.nx] = B_lin.dot(np.array
([v_ref, kappa_ref])) - f
# Constrain maximum speed based on predicted car curvature
vmax_dyn = np.sqrt(self.ay_max / (np.abs(kappa_pred[n]) + 1e-12))
if vmax_dyn < umax_dyn[self.nu*n]:
umax_dyn[self.nu*n] = vmax_dyn
# Compute dynamic constraints on e_y
ub, lb, _ = self.model.reference_path.update_path_constraints(
self.model.wp_id+1, self.N, 2*self.model.safety_margin,
self.model.safety_margin)
xmin_dyn[0] = self.model.spatial_state.e_y
xmax_dyn[0] = self.model.spatial_state.e_y
xmin_dyn[self.nx::self.nx] = lb
xmax_dyn[self.nx::self.nx] = ub
# Set reference for state as center-line of drivable area
xr[self.nx::self.nx] = (lb + ub) / 2
# Get equality matrix
Ax = sparse.kron(sparse.eye(self.N + 1),
-sparse.eye(self.nx)) + sparse.csc_matrix(A)
Bu = sparse.csc_matrix(B)
Aeq = sparse.hstack([Ax, Bu])
# Get inequality matrix
Aineq = sparse.eye((self.N + 1) * self.nx + self.N * self.nu)
# Combine constraint matrices
A = sparse.vstack([Aeq, Aineq], format='csc')
# Get upper and lower bound vectors for equality constraints
lineq = np.hstack([xmin_dyn,
np.kron(np.ones(self.N), umin)])
uineq = np.hstack([xmax_dyn, umax_dyn])
# Get upper and lower bound vectors for inequality constraints
x0 = np.array(self.model.spatial_state[:])
leq = np.hstack([-x0, uq])
ueq = leq
# Combine upper and lower bound vectors
l = np.hstack([leq, lineq])
u = np.hstack([ueq, uineq])
# Set cost matrices
P = sparse.block_diag([sparse.kron(sparse.eye(self.N), self.Q), self.QN,
sparse.kron(sparse.eye(self.N), self.R)], format='csc')
q = np.hstack(
[-np.tile(np.diag(self.Q.A), self.N) * xr[:-self.nx],
-self.QN.dot(xr[-self.nx:]),
-np.tile(np.diag(self.R.A), self.N) * ur])
# Initialize optimizer
self.optimizer = osqp.OSQP()
self.optimizer.setup(P=P, q=q, A=A, l=l, u=u, verbose=False)
def solve_loop(qp_matrices, problem, nsim, solver='osqp'):
"""
Solve MPC loop
"""
# Shorter name for qp_matrices
qp = qp_matrices
# Get dimensions
(nx, nu) = problem.B.shape
N = problem.N
print('N = %d and solver %s' % (N, solver))
# Initialize time and number of iterations vectors
time = np.zeros(nsim)
niter = np.zeros(nsim)
if solver == 'osqp':
# Construct qp matrices
if len(problem.xmin) > 0:
# If the problem has state constraints
Aosqp = spa.vstack([
qp.A,
spa.eye((N + 1) * nx + N * nu),
]).tocsc()
else:
Aosqp = spa.vstack([
qp.A,
spa.hstack(
[spa.csc_matrix((N * nu, (N + 1) * nx)),
spa.eye(N * nu)]),
]).tocsc()
losqp = np.hstack([qp.l, qp.lx])
uosqp = np.hstack([qp.u, qp.ux])
# Initial state
x0 = problem.x0
# Setup OSQP
m = osqp.OSQP()
m.setup(
qp.P,
qp.q,
Aosqp,
losqp,
uosqp,
# auto_rho=False,
auto_rho=True,
max_iter=2500,
scaling=True,
scaling_iter=50,
polish=False,
verbose=False)
for i in range(nsim):
# Solve with osqp
res = m.solve()
# Save time and number of iterations
time[i] = res.info.run_time
niter[i] = res.info.iter
# Check if status is correct
status = res.info.status_val
if status != m.constant('OSQP_SOLVED'):
# # Dump file to 'bad_convergence/data'folder
# import pickle
# problem = {'P': qp.P,
# 'q': qp.q,
# 'A': Aosqp,
# 'l': losqp,
# 'u': uosqp}
# with open('bad_convergence/data/%s.pickle' % 'helicopter_scaling_large', 'wb') as f:
# pickle.dump(problem, f)
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# Apply first control input to the plant
u = res.x[-N * nu:-(N - 1) * nu]
x0 = problem.A.dot(x0) + problem.B.dot(u)
# Update linear constraints
if len(problem.tmin) > 0:
losqp = np.hstack([b(x0, nx, N), problem.tmin, qp.lx])
uosqp = np.hstack([b(x0, nx, N), problem.tmax, qp.ux])
else:
losqp = np.hstack([b(x0, nx, N), qp.lx])
uosqp = np.hstack([b(x0, nx, N), qp.ux])
m.update(l=losqp, u=uosqp)
elif solver == 'osqp_coldstart':
# Construct qp matrices
if len(problem.xmin) > 0:
# If the problem has state constraints
Aosqp = spa.vstack([
qp.A,
spa.eye((N + 1) * nx + N * nu),
]).tocsc()
else:
Aosqp = spa.vstack([
qp.A,
spa.hstack(
[spa.csc_matrix((N * nu, (N + 1) * nx)),
spa.eye(N * nu)]),
]).tocsc()
losqp = np.hstack([qp.l, qp.lx])
uosqp = np.hstack([qp.u, qp.ux])
# Initial state
x0 = problem.x0
# Setup OSQP
m = osqp.OSQP()
m.setup(
qp.P,
qp.q,
Aosqp,
losqp,
uosqp,
warm_start=False,
auto_rho=True,
# auto_rho=False,
rho=0.1,
max_iter=2500,
scaling_iter=50,
polish=False,
verbose=False)
for i in range(nsim):
# Solve with osqp
res = m.solve()
# Save time and number of iterations
time[i] = res.info.run_time
niter[i] = res.info.iter
# Check if status is correct
status = res.info.status_val
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# Apply first control input to the plant
u = res.x[-N * nu:-(N - 1) * nu]
x0 = problem.A.dot(x0) + problem.B.dot(u)
# Update linear constraints
if len(problem.tmin) > 0:
losqp = np.hstack([b(x0, nx, N), problem.tmin, qp.lx])
uosqp = np.hstack([b(x0, nx, N), problem.tmax, qp.ux])
else:
losqp = np.hstack([b(x0, nx, N), qp.lx])
uosqp = np.hstack([b(x0, nx, N), qp.ux])
m.update(l=losqp, u=uosqp)
elif solver == 'osqp_no_caching':
# Construct qp matrices
if len(problem.xmin) > 0:
# If the problem has state constraints
Aosqp = spa.vstack([
qp.A,
spa.eye((N + 1) * nx + N * nu),
]).tocsc()
else:
Aosqp = spa.vstack([
qp.A,
spa.hstack(
[spa.csc_matrix((N * nu, (N + 1) * nx)),
spa.eye(N * nu)]),
]).tocsc()
losqp = np.hstack([qp.l, qp.lx])
uosqp = np.hstack([qp.u, qp.ux])
# Initial state
x0 = problem.x0
for i in range(nsim):
# Setup OSQP
m = osqp.OSQP()
m.setup(
qp.P,
qp.q,
Aosqp,
losqp,
uosqp,
warm_start=False,
auto_rho=True,
# auto_rho=False,
rho=0.1,
max_iter=2500,
scaling_iter=50,
polish=False,
verbose=False)
# Solve
res = m.solve()
# Save time and number of iterations
time[i] = res.info.run_time
niter[i] = res.info.iter
# Check if status is correct
status = res.info.status_val
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# Apply first control input to the plant
u = res.x[-N * nu:-(N - 1) * nu]
x0 = problem.A.dot(x0) + problem.B.dot(u)
# Update linear constraints
if len(problem.tmin) > 0:
losqp = np.hstack([b(x0, nx, N), problem.tmin, qp.lx])
uosqp = np.hstack([b(x0, nx, N), problem.tmax, qp.ux])
else:
losqp = np.hstack([b(x0, nx, N), qp.lx])
uosqp = np.hstack([b(x0, nx, N), qp.ux])
m.update(l=losqp, u=uosqp)
elif solver == 'qpoases':
n_dim = qp.P.shape[0] # Number of variables
m_dim = qp.A.shape[0] # Number of constraints without bounds
# Initialize qpoases and set options
qpoases_m = qpoases.PyQProblem(n_dim, m_dim)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qpoases_m.setOptions(options)
# Construct bounds for qpoases
lx = np.append(-np.inf * np.ones((N + 1) * nx), qp.lx)
ux = np.append(np.inf * np.ones((N + 1) * nx), qp.ux)
# Setup matrix P and A
P = np.ascontiguousarray(qp.P.todense())
A = np.ascontiguousarray(qp.A.todense())
# Initial state
x0 = problem.x0
# RHS of the linear equality constraints
lqpoases = qp.l
uqpoases = qp.u
for i in range(nsim):
# Reset cpu time
qpoases_cpu_time = np.array([60.])
# Reset number of of working set recalculations
nWSR = np.array([1e6])
if i == 0:
# First iteration
res_qpoases = qpoases_m.init(P, np.ascontiguousarray(qp.q), A,
np.ascontiguousarray(lx),
np.ascontiguousarray(ux),
np.ascontiguousarray(lqpoases),
np.ascontiguousarray(uqpoases),
nWSR, qpoases_cpu_time)
else:
# Solve new hot started problem
res_qpoases = qpoases_m.hotstart(
np.ascontiguousarray(qp.q), np.ascontiguousarray(lx),
np.ascontiguousarray(ux), np.ascontiguousarray(lqpoases),
np.ascontiguousarray(uqpoases), nWSR, qpoases_cpu_time)
if res_qpoases != 0:
raise ValueError('qpoases did not solve the problem!')
# Save time and number of iterations
time[i] = qpoases_cpu_time[0]
niter[i] = nWSR[0]
# Get qpoases solution
sol_qpoases = np.zeros((N + 1) * nx + N * nu)
qpoases_m.getPrimalSolution(sol_qpoases)
# Apply first control input to the plant
u = sol_qpoases[-N * nu:-(N - 1) * nu]
x0 = problem.A.dot(x0) + problem.B.dot(u)
# Update linear constraints
if len(problem.tmin) > 0:
lqpoases = np.hstack([b(x0, nx, N), problem.tmin])
uqpoases = np.hstack([b(x0, nx, N), problem.tmax])
else:
lqpoases = np.hstack([b(x0, nx, N)])
uqpoases = np.hstack([b(x0, nx, N)])
elif solver == 'gurobi' or solver == 'mosek':
# Construct qp matrices
if len(problem.xmin) > 0:
# If the problem has state constraints
Agurobi = spa.vstack([
qp.A,
spa.eye((N + 1) * nx + N * nu),
]).tocsc()
else:
Agurobi = spa.vstack([
qp.A,
spa.hstack(
[spa.csc_matrix((N * nu, (N + 1) * nx)),
spa.eye(N * nu)]),
]).tocsc()
lgurobi = np.hstack([qp.l, qp.lx])
ugurobi = np.hstack([qp.u, qp.ux])
# Initial state
x0 = problem.x0
for i in range(nsim):
# Solve with gurobi
prob = mpbpy.QuadprogProblem(qp.P, qp.q, Agurobi, lgurobi, ugurobi)
if solver == 'gurobi':
res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
else:
res = prob.solve(solver=mpbpy.MOSEK, verbose=False)
# Save time and number of iterations
time[i] = res.cputime
niter[i] = res.total_iter
# Check if status is correct
status = res.status
if status != 'optimal':
import ipdb
ipdb.set_trace()
raise ValueError('Gurobi did not solve the problem!')
# Apply first control input to the plant
u = res.x[-N * nu:-(N - 1) * nu]
x0 = problem.A.dot(x0) + problem.B.dot(u)
# Update QP problem
if len(problem.tmin) > 0:
lgurobi = np.hstack([b(x0, nx, N), problem.tmin, qp.lx])
ugurobi = np.hstack([b(x0, nx, N), problem.tmax, qp.ux])
else:
lgurobi = np.hstack([b(x0, nx, N), qp.lx])
ugurobi = np.hstack([b(x0, nx, N), qp.ux])
else:
raise ValueError('Solver not understood')
# Return statistics
return utils.Statistics(time), utils.Statistics(niter)
def __init__(self,
linear_dynamics,
N,
dt,
umin,
umax,
xmin,
xmax,
Q,
R,
QN,
xr,
plotMPC=False,
plotMPC_filename="",
lifting=False,
edmd_object=Edmd(),
name="noname",
soft=False,
D=None):
"""Create an MPC Controller object.
Sizes:
- N: number of timesteps for predictions
- Nqd: number of timesteps of the desired trajectory
- nx: number of states (original or lifted)
- ns: number or original states
- nu: number of control inputs
Inputs:
- initilized linear_dynamics, LinearSystemDynamics object. It takes Ac and Bc from it.
- number of timesteps, N, integer
- time step, dt, float
- minimum control, umin, numpy 1d array [nu,]
- maximum control, umax, numpy 1d array [nu,]
- minimum state, xmin, numpy 1d array [ns,]
- maximum state, xmax, numpy 1d array [ns,]
- state cost matrix Q, sparse numpy 2d array [ns,ns]. In practice it is always diagonal.
- control cost matrix, R, sparse numpy 2d array [nu,nu]. In practice it is always diagonal.
- final state cost matrix, QN, sparse numpy 2d array [ns,ns]. In practice it is always diagonal.
- reference state trajectory, xr, numpy 2d array [ns,Nqd] OR numpy 1d array [ns,]
(Optional)
- flag to plot MPC thoughts, plotMPC=False, boolean
- filename to save the previosu plot, plotMPC_filename="", string
- flag to use or not lifting, lifting=False, boolean
- object to store the eDMD data, edmd_object=Edmd(). It has been initialized. s
"""
Controller.__init__(self, linear_dynamics)
# Load arguments
Ac, Bc = linear_dynamics.linear_system()
[nx, nu] = Bc.shape
ns = xr.shape[0]
self.dt = dt
self.plotMPC = plotMPC
self.plotMPC_filename = plotMPC_filename
self.verbose = False
self.q_d = xr
self.Q = Q
self.R = R
self.lifting = lifting
self.soft = soft
self.nu = nu
self.nx = nx
self.ns = ns
#Discretize dynamics:
self.dt = dt
if lifting:
self.C = edmd_object.C
self.edmd_object = edmd_object
else:
self.C = sparse.eye(ns)
lin_model_d = sp.signal.cont2discrete((Ac, Bc, self.C, zeros((ns, 1))),
dt)
Ad = sparse.csc_matrix(
lin_model_d[0]) #TODO: If bad behavior, delete this
Bd = sparse.csc_matrix(
lin_model_d[1]) #TODO: If bad behavior, delete this
# Total desired path
if self.q_d.ndim == 2:
self.Nqd = self.q_d.shape[1]
xr = self.q_d[:, :N]
# Prediction horizon
self.N = N
x0 = np.zeros(nx)
self.run_time = np.zeros([
0,
])
self.xr = self.q_d[:, :N]
Rbd = sparse.kron(sparse.eye(N), R)
Qbd = sparse.kron(sparse.eye(N), Q)
Bbd = block_diag(Bd, nu).tocoo()
# Check Xmin and Xmax
if xmin.shape[
0] == ns and xmin.ndim == 1: # it is a single vector we tile it
x_min_flat = np.kron(np.ones(N), xmin)
x_max_flat = np.kron(np.ones(N), xmax)
elif xmin.shape[0] == ns * N: # if it is a long vector it is ok
x_min_flat = xmin
x_max_flat = xmax
elif xmin.shape[0] == ns and xmin.shape[
1] == N: # if it is a block we flatten it
x_min_flat = np.reshape(xmin, (N * ns, ), order='F')
x_max_flat = np.reshape(xmax, (N * ns, ), order='F')
else:
raise ValueError('xmin has wrong dimensions. xmin shape={}'.format(
xmin.shape))
self.x_min_flat = x_min_flat
self.x_max_flat = x_max_flat
# Check Umin and Umax
if umin.shape[0] == nu and umin.ndim == 1:
u_min_flat = np.kron(np.ones(N), umin)
u_max_flat = np.kron(np.ones(N), umax)
elif umin.shape[0] == nu * N:
u_min_flat = umin
u_max_flat = umax
elif umin.shape[0] == nu and umin.shape[1] == N:
u_min_flat = np.reshape(umin, (N * nu, ), order='F')
u_max_flat = np.reshape(umax, (N * nu, ), order='F')
else:
raise ValueError('umin has wrong dimensions. Umin shape={}'.format(
umin.shape))
self.u_min_flat = u_min_flat
self.u_max_flat = u_max_flat
#! GET a & b
# Write B:
diag_AkB = Bd
data_list = Bbd.data
row_list = Bbd.row
col_list = Bbd.col
B = sparse.coo_matrix
for i in range(N):
if i < N - 1:
AkB_bd_temp = block_diag(diag_AkB, N - i)
else:
AkB_bd_temp = diag_AkB.tocoo()
data_list = np.hstack([data_list, AkB_bd_temp.data])
row_list = np.hstack([
row_list, AkB_bd_temp.row + np.full(
(AkB_bd_temp.row.shape[0], ), nx * i)
])
col_list = np.hstack([col_list, AkB_bd_temp.col])
diag_AkB = Ad.dot(diag_AkB)
B = sparse.coo_matrix((data_list, (row_list, col_list)),
shape=(N * nx, N * nu))
a = Ad.copy()
Ak = Ad.copy()
for i in range(N - 1):
Ak = Ak.dot(Ad)
a = sparse.vstack([a, Ak])
self.a = a
self.B = B
# check_ab = True
# if check_ab:
# x0 = np.linspace(-5,40,nx)
# x00 = np.linspace(-5,40,nx)
# # Store data Init
# nsim = N
# xst = np.zeros((nx,nsim))
# ust = np.zeros((nu,nsim))
# # Simulate in closed loop
# for i in range(nsim):
# # Fake pd controller
# ctrl = np.zeros(nu,) #np.random.rand(nu,)
# x0 = Ad.dot(x0) + Bd.dot(ctrl)
# # Store Data
# xst[:,i] = x0
# ust[:,i] = ctrl
# x_dense = np.reshape(a @ x00 + B @ (ust.flatten('F')),(N,nx)).T
# plt.figure()
# plt.subplot(2,1,1)
# for i in range(nx):
# plt.plot(range(nsim),xst[i,:],'d',label="sim "+str(i))
# plt.plot(range(nsim),x_dense[i,:],'d',label="ax+bu "+str(i))
# plt.xlabel('Time(s)')
# plt.grid()
# plt.legend()
# plt.subplot(2,1,2)
# for i in range(nu):
# plt.plot(range(nsim),ust[i,:],label=str(i))
# plt.xlabel('Time(s)')
# plt.grid()
# plt.legend()
# plt.savefig("AB_check_for_"+name+".png",bbox_inches='tight')
# plt.close()
# Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
if (self.lifting):
# Compute Block Diagonal elements
self.Cbd = sparse.kron(sparse.eye(N), self.C)
CQCbd = self.Cbd.T @ Qbd @ self.Cbd
self.CtQ = self.C.T @ Q
Cbd = self.Cbd
P = Rbd + B.T @ CQCbd @ B
self.BTQbda = B.T @ CQCbd @ a
Aineq_x = Cbd @ B
xrQB = B.T @ np.reshape(self.CtQ.dot(xr), (N * nx, ), order='F')
l = np.hstack([x_min_flat - Cbd @ a @ x0, u_min_flat])
u = np.hstack([x_max_flat - Cbd @ a @ x0, u_max_flat])
else:
# - quadratic objective
P = Rbd + B.T @ Qbd @ B
self.BTQbda = B.T @ Qbd @ a
xrQB = B.T @ np.reshape(Q.dot(xr), (N * nx, ), order='F')
Aineq_x = B
l = np.hstack([x_min_flat - a @ x0, u_min_flat])
u = np.hstack([x_max_flat - a @ x0, u_max_flat])
x0aQb = self.BTQbda @ x0
self.Cbda = Cbd @ a
self.BQxr = self.B.T @ np.reshape(self.CtQ.dot(self.xr), (N * nx, ),
order='F')
q = x0aQb - xrQB
Aineq_u = sparse.eye(N * nu)
A = sparse.vstack([Aineq_x, Aineq_u]).tocsc()
if soft:
Pdelta = sparse.kron(sparse.eye(N), D)
P = sparse.block_diag([P, Pdelta])
qdelta = np.zeros(N * ns)
q = np.hstack([q, qdelta])
Adelta = sparse.csc_matrix(
np.vstack([np.eye(N * ns),
np.zeros((N * nu, N * ns))]))
A = sparse.hstack([A, Adelta])
# plot_matrices = True
# if plot_matrices:
# #! Visualize Matrices
# fig = plt.figure()
# fig.suptitle("QP Matrices to solve MP in dense form. N={}, nx={}, nu={}".format(N,nx,nu),fontsize=20)
# plt.subplot(2,4,1,xlabel="Ns*(N+1)", ylabel="Ns*(N+1)")
# plt.imshow(a.toarray(), interpolation='nearest', cmap=cm.Greys_r)
# plt.title("a in $x=ax_0+bu$")
# plt.subplot(2,4,2,xlabel="Ns*(N+1)", ylabel="Nu*N")
# plt.imshow(B.toarray(), interpolation='nearest', cmap=cm.Greys_r)
# plt.title("b in $x=ax_0+bu$")
# plt.subplot(2,4,3,xlabel="ns*(N+1) + ns*(N+1) + nu*N", ylabel="Ns*(N+1)+Nu*N")
# plt.imshow(A.toarray(), interpolation='nearest', cmap=cm.Greys_r)
# plt.title("A total in $l\\leq Ax \\geq u$")
# plt.subplot(2,4,4)
# plt.imshow(P.toarray(), interpolation='nearest', cmap=cm.Greys_r)
# plt.title("P in $J=u^TPu+q^Tu$")
# plt.subplot(2,4,5)
# plt.imshow(Qbd.toarray(), interpolation='nearest', cmap=cm.Greys_r)
# plt.title("Qbd")
# #! Visualize Vectors
# plt.subplot(2,4,6)
# plt.plot(l)
# plt.title('l in $l\\leq Ax \\geq u$')
# plt.grid()
# plt.subplot(2,4,7)
# plt.plot(u)
# plt.title("l in $l\\leq Ax \\geq u$")
# plt.grid()
# plt.subplot(2,4,8)
# plt.plot(q)
# plt.title("q in $J=u^TPu+q^Tu$")
# plt.grid()
# plt.tight_layout()
# plt.savefig("MPC_matrices_for_"+name+".png",bbox_inches='tight')
# plt.close()
#plt.show()
self.osqp_size = P.shape[0]
# Create an OSQP object
self.prob = osqp.OSQP()
# Setup workspace
self.prob.setup(P=P.tocsc(),
q=q,
A=A,
l=l,
u=u,
warm_start=True,
verbose=self.verbose)
if self.plotMPC:
# Figure to plot MPC thoughts
self.fig, self.axs = plt.subplots(self.ns + self.nu)
if nx == 4:
ylabels = [
'$x$', '$\\theta$', '$\\dot{x}$', '$\\dot{\\theta}$'
]
else:
ylabels = [str(i) for i in range(nx)]
for ii in range(self.ns):
self.axs[ii].set(xlabel='Time(s)', ylabel=ylabels[ii])
self.axs[ii].grid()
for ii in range(self.ns, self.ns + self.nu):
self.axs[ii].set(xlabel='Time(s)', ylabel='u')
self.axs[ii].grid()
def solve_problem(qp_matrices, n_prob, solver='osqp'):
"""
Solve Huber fitting problem
"""
# Shorter name for qp_matrices
qp = qp_matrices
# Get dimensions
m = int(len(qp.lx) / 2)
n = len(qp.q) - 2 * m
print('n = %d and solver %s' % (n, solver))
# Initialize time vector
time = np.zeros(n_prob)
# Initialize number of iterations vector
niter = np.zeros(n_prob)
if solver == 'osqp':
# Construct qp matrices
Aosqp = spa.vstack(
[qp.A,
spa.hstack([spa.csc_matrix((2 * m, n)),
spa.eye(2 * m)])]).tocsc()
losqp = np.hstack([qp.l, qp.lx])
uosqp = np.hstack([qp.u, qp.ux])
for i in range(n_prob):
# Setup OSQP
m = osqp.OSQP()
m.setup(qp.P,
qp.q,
Aosqp,
losqp,
uosqp,
auto_rho=True,
polish=False,
verbose=False)
# Solve
results = m.solve()
x = results.x
status = results.info.status_val
niter[i] = results.info.iter
time[i] = results.info.run_time
# Check if status correct
if status != m.constant('OSQP_SOLVED'):
import ipdb
ipdb.set_trace()
raise ValueError('OSQP did not solve the problem!')
# DEBUG
# solve with gurobi
# import mathprogbasepy as mpbpy
# prob = mpbpy.QuadprogProblem(qp.P, qp.q, Aosqp, losqp, uosqp)
# res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
# print('Norm difference OSQP-GUROBI %.3e' %
# np.linalg.norm(x - res.x))
# import ipdb; ipdb.set_trace()
elif solver == 'qpoases':
for i in range(n_prob):
n_dim = qp.P.shape[0] # Number of variables
m_dim = qp.A.shape[0] # Number of constraints without bounds
# Initialize qpoases and set options
qpoases_m = qpoases.PyQProblem(n_dim, m_dim)
options = qpoases.PyOptions()
options.printLevel = qpoases.PyPrintLevel.NONE
qpoases_m.setOptions(options)
# Construct bounds for qpoases
lx = np.append(-np.inf * np.ones(n), qp.lx)
ux = np.append(np.inf * np.ones(n), qp.ux)
# Setup matrix P and A
P = np.ascontiguousarray(qp.P.todense())
A = np.ascontiguousarray(qp.A.todense())
# Reset cpu time
qpoases_cpu_time = np.array([10.])
# Reset number of working set recalculations
nWSR = np.array([1000])
# Solve
res_qpoases = qpoases_m.init(P, np.ascontiguousarray(qp.q), A,
np.ascontiguousarray(lx),
np.ascontiguousarray(ux),
np.ascontiguousarray(qp.l),
np.ascontiguousarray(qp.u), nWSR,
qpoases_cpu_time)
# if res_qpoases != 0:
# raise ValueError('qpoases did not solve the problem!')
# Save time and number of iterations
time[i] = qpoases_cpu_time[0]
niter[i] = nWSR[0]
elif solver == 'gurobi':
for i in range(n_prob):
# Construct qp matrices
Agurobi = spa.vstack([
qp.A,
spa.hstack([spa.csc_matrix((2 * m, n)),
spa.eye(2 * m)])
]).tocsc()
lgurobi = np.hstack([qp.l, qp.lx])
ugurobi = np.hstack([qp.u, qp.ux])
# Solve with gurobi
prob = mpbpy.QuadprogProblem(qp.P, qp.q, Agurobi, lgurobi, ugurobi)
res = prob.solve(solver=mpbpy.GUROBI, verbose=False)
niter[i] = res.total_iter
time[i] = res.cputime
elif solver == 'mosek':
for i in range(n_prob):
# Construct qp matrices
Amosek = spa.vstack([
qp.A,
spa.hstack([spa.csc_matrix((2 * m, n)),
spa.eye(2 * m)])
]).tocsc()
lmosek = np.hstack([qp.l, qp.lx])
umosek = np.hstack([qp.u, qp.ux])
# Solve with mosek
prob = mpbpy.QuadprogProblem(qp.P, qp.q, Amosek, lmosek, umosek)
res = prob.solve(solver=mpbpy.MOSEK, verbose=False)
niter[i] = res.total_iter
time[i] = res.cputime
elif solver == 'ecos':
for i in range(n_prob):
# Model with CVXPY
# minimize 1/2 u.T * u + np.ones(m).T * v
# subject to -u - v <= Ax - b <= u + v
# 0 <= u <= 1
# v >= 0
n_var = qp.A_huber.shape[1]
m_var = qp.b_huber.shape[0]
x = cvxpy.Variable(n_var)
u = cvxpy.Variable(m_var)
v = cvxpy.Variable(m_var)
objective = cvxpy.Minimize(.5 *
cvxpy.quad_form(u, spa.eye(m_var)) +
np.ones(m_var) * v)
constraints = [
-u - v <= qp.A_huber * x - qp.b_huber,
qp.A_huber * x - qp.b_huber <= u + v, 0 <= u, u <= 1, v >= 0
]
problem = cvxpy.Problem(objective, constraints)
problem.solve(solver=cvxpy.ECOS, verbose=False)
# DEBUG: solve with MOSEK
# Amosek = spa.vstack([
# qp.A,
# spa.hstack([spa.csc_matrix((2*m, n)), spa.eye(2*m)])
# ]).tocsc()
# lmosek = np.hstack([qp.l, qp.lx])
# umosek = np.hstack([qp.u, qp.ux])
#
# # Solve with mosek
# prob = mpbpy.QuadprogProblem(qp.P, qp.q, Amosek, lmosek, umosek)
# res = prob.solve(solver=mpbpy.MOSEK, verbose=False)
# x_mosek = res.x[:n_var]
# Obtain time and number of iterations
time[i] = problem.solver_stats.setup_time + \
problem.solver_stats.solve_time
niter[i] = problem.solver_stats.num_iters
else:
raise ValueError('Solver not understood')
# Return statistics
return utils.Statistics(time), utils.Statistics(niter)
def init_problem(self):
# constraints
umin = self.input_constraints["umin"]
umax = self.input_constraints["umax"]
xmin = self.state_constraints["xmin"]
xmax = self.state_constraints["xmax"]
# LTV systems
# huge matrices that consist of all matrices in a given horizon (N)
A = np.zeros((self.nx * (self.N + 1), self.nx * (self.N + 1)))
B = np.zeros((self.nx * (self.N + 1), self.nu * (self.N)))
x_ref = np.zeros(self.nx * (self.N + 1))
u_ref = np.zeros(self.nu * self.N)
# offset for equality constraints
u_eq = np.zeros(self.N * self.nx)
# dynamic state and input constraints
xmin_dyn = np.kron(np.ones(self.N + 1), xmin)
print("xmin_dyn dimension ", xmin_dyn.shape)
xmax_dyn = np.kron(np.ones(self.N + 1), xmax)
umax_dyn = np.kron(np.ones(self.N), umax)
# derive predicted curvature from last control
# kappa = tan(delta) / length
kappa_pred = (np.tan(
np.array(self.current_control[3::] + self.current_control[-1::])) /
self.model.length)
# fill the information over entire horizon
for n in range(self.N):
# extract information from current waypoint
current_waypoint = self.model.reference_path.get_waypoint(
self.model.wp_id + n)
next_waypoint = self.model.reference_path.get_waypoint(
self.model.wp_id + n + 1)
# Previous reference output
# delta_s = next_waypoint - current_waypoint
# kappa_ref = current_waypoint.kappa
v_ref = current_waypoint.v_ref
psi_ref = current_waypoint.psi
# TODO: reference values below haven't created yet
delta_ref = current_waypoint.delta
# get linearized LTV model
# TODO: different model, different states
A_lin, B_lin = self.model.linearize(v_ref, psi_ref, delta_ref)
A[(n + 1) * self.nx:(n + 2) * self.nx,
n * self.nx:(n + 1) * self.nx] = A_lin
B[(n + 1) * self.nx:(n + 2) * self.nx,
n * self.nu:(n + 1) * self.nu] = B_lin
# TODO: 2 inputs have changed
u_ref[n * self.nu:(n + 1) * self.nu] = np.array([v_ref, delta_ref])
# compute equality constraint offset
# TODO: dunno what this is
u_eq[n * self.nx:(n + 1) * self.nx] = B_lin.dot(
np.array([v_ref, delta_ref]))
# maximum speed limited by predicted car curvature
vmax_dyn = np.sqrt(self.ay_max / (np.abs(kappa_pred[n] + 1e-12)))
if vmax_dyn < umax_dyn[self.nu * n]:
umax_dyn[self.nu * n] = vmax_dyn
# compute dynamic constraints on states
upp_b, low_b, _ = self.model.reference_path.update_path_constraints(
self.model.wp_id + 1,
self.N,
2 * self.model.safety_margin,
self.model.safety_margin,
)
# TODO: the constraints should place on x,y instead of e_y
xmin_dyn[0], xmin_dyn[1] = (
self.model.temporal_state.x,
self.model.temporal_state.y,
)
xmax_dyn[0], xmax_dyn[1] = (
self.model.temporal_state.x,
self.model.temporal_state.y,
)
for i in range(self.N):
xmin_dyn[self.nx + i * self.nx] = low_b[0 + i * self.nx]
xmin_dyn[self.nx + i * self.nx + 1] = low_b[1 + i * self.nx]
xmax_dyn[self.nx + i * self.nx] = upp_b[0 + i * self.nx] + 0.000001
xmax_dyn[self.nx + i * self.nx + 1] = upp_b[1 +
i * self.nx] = 0.000001
# reference state = middle line of free space
x_ref[self.nx + i * self.nx] = (low_b[0 + i * self.nx] +
upp_b[0 + i * self.nx]) / 2
x_ref[self.nx + i * self.nx + 1] = (low_b[0 + i * self.nx + 1] +
upp_b[0 + i * self.nx + 1]) / 2
"""
form an QP problem using the format of OSQP
we need P, q, A, l, u
for more infromation, visit https://osqp.org/docs/examples/setup-and-solve.html
"""
################################
# P: matrix for quadratic term #
# q: matrix for linear term #
################################
P = sparse.block_diag(
[
sparse.kron(sparse.eye(self.N), self.Q),
self.QN,
sparse.kron(sparse.eye(self.N), self.R),
],
format="csc",
)
# TODO: dunno how to construct
q = np.hstack([
-np.tile(np.diag(self.Q.A), self.N) * x_ref[:-self.nx],
-self.QN.dot(x_ref[-self.nx:]),
-np.tile(np.diag(self.R.A), self.N) * u_ref,
])
#################################
# A: system matrix in osqp form #
#################################
# equality part
Ax = sparse.kron(sparse.eye(self.N + 1),
-sparse.eye(self.nx)) + sparse.csc_matrix(A)
Bu = sparse.csc_matrix(B)
A_eq = sparse.hstack([Ax, Bu])
# inequality matrix
A_ineq = sparse.eye((self.N + 1) * self.nx + self.N * self.nu)
A = sparse.vstack([A_eq, A_ineq], format="csc")
#####################################
# l, u: lower bound and upper bound #
#####################################
# equality part: just for the format
x0 = np.array(self.model.temporal_state[:])
low_eq = np.hstack([-x0, u_eq])
upp_eq = low_eq # format reason
# inequality part
low_ineq = np.hstack([xmin_dyn, np.kron(np.ones(self.N), umin)])
upp_ineq = np.hstack([xmax_dyn, umax_dyn])
# results
l = np.hstack([low_eq, low_ineq])
u = np.hstack([upp_eq, upp_ineq])
#####################################
# Solve the problem #
#####################################
self.optimizer = osqp.OSQP()
self.optimizer.setup(P=P, q=q, A=A, l=l, u=u, verbose=False)
def __init__(self, linear_dynamics, N, dt, umin, umax, xmin, xmax, Q, R, QN, xr, plotMPC=False, plotMPC_filename="",lifting=False, edmd_object=Edmd()):
"""__init__ Create an MPC controller
Arguments:
linear_dynamics {dynamical sytem} -- it contains the A and B matrices in continous time
N {integer} -- number of timesteps
dt {float} -- time step in seconds
umin {numpy array [Nu,]} -- minimum control bound
umax {numpy array [Nu,]} -- maximum control bound
xmin {numpy array [Ns,]} -- minimum state bound
xmax {numpy array [Ns,]} -- maximum state bound
Q {numpy array [Ns,Ns]} -- state cost matrix
R {numpy array [Nu,Nu]} -- control cost matrix
QN {numpy array [Ns,]} -- final state cost
xr {numpy array [Ns,]} -- reference trajectory
Keyword Arguments:
plotMPC {bool} -- flag to plot results (default: {False})
plotMPC_filename {str} -- plotting filename (default: {""})
lifting {bool} -- flag to use state lifting (default: {False})
edmd_object {edmd object} -- lifting object. It contains projection matrix and lifting function (default: {Edmd()})
"""
Controller.__init__(self, linear_dynamics)
# Load arguments
Ac, Bc = linear_dynamics.linear_system()
[nx, nu] = Bc.shape
self.dt = dt
self._osqp_Ad = sparse.eye(nx)+Ac*self.dt
self._osqp_Bd = Bc*self.dt
self.plotMPC = plotMPC
self.plotMPC_filename = plotMPC_filename
self.q_d = xr
self.ns = xr.shape[0]
self.Q = Q
self.lifting = lifting
self.nu = nu
self.nx = nx
# Total desired path
if self.q_d.ndim==2:
self.Nqd = self.q_d.shape[1]
xr = self.q_d[:,:N+1]
# Prediction horizon
self.N = N
x0 = np.zeros(nx)
# Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
if (self.lifting):
# Load eDMD objects
self.C = edmd_object.C
self.edmd_object = edmd_object
# - quadratic objective
CQC = sparse.csc_matrix(np.transpose(edmd_object.C).dot(Q.dot(edmd_object.C)))
CQNC = sparse.csc_matrix(np.transpose(edmd_object.C).dot(QN.dot(edmd_object.C)))
P = sparse.block_diag([sparse.kron(sparse.eye(N), CQC), CQNC,
sparse.kron(sparse.eye(N), R)]).tocsc()
# - linear objective
QCT = np.transpose(Q.dot(edmd_object.C))
QNCT = np.transpose(QN.dot(edmd_object.C))
if (xr.ndim==1):
q = np.hstack([np.kron(np.ones(N), -QCT.dot(xr)), -QNCT.dot(xr), np.zeros(N*nu)])
elif (xr.ndim==2):
q = np.hstack([np.reshape(-QCT.dot(xr),((N+1)*nx,),order='F'), np.zeros(N*nu)])
# - input and state constraints
Aineq = sparse.block_diag([edmd_object.C for i in range(N+1)]+[np.eye(N*nu)])
else:
# - quadratic objective
P = sparse.block_diag([sparse.kron(sparse.eye(N), Q), QN,
sparse.kron(sparse.eye(N), R)]).tocsc()
# - linear objective
if (xr.ndim==1):
q = np.hstack([np.kron(np.ones(N), -Q.dot(xr)), -QN.dot(xr), np.zeros(N*nu)])
elif (xr.ndim==2):
q = np.hstack([np.reshape(-Q.dot(xr),((N+1)*nx,),order='F'), np.zeros(N*nu)]) #TODO: Check if reshape is reshaping in the expected order
# - input and state constraints
Aineq = sparse.eye((N+1)*nx + N*nu)
# - linear dynamics
Ax = sparse.kron(sparse.eye(N+1),-sparse.eye(nx)) + sparse.kron(sparse.eye(N+1, k=-1), self._osqp_Ad)
Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]), self._osqp_Bd)
Aeq = sparse.hstack([Ax, Bu])
leq = np.hstack([-x0, np.zeros(N*nx)])
lineq = np.hstack([np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin)])
uineq = np.hstack([np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax)])
ueq = leq
self._osqp_q = q
A = sparse.vstack([Aeq, Aineq]).tocsc()
self._osqp_l = np.hstack([leq, lineq])
self._osqp_u = np.hstack([ueq, uineq])
# Create an OSQP object
self.prob = osqp.OSQP()
# Setup workspace
self.prob.setup(P, q, A, self._osqp_l, self._osqp_u, warm_start=True, verbose=False)
if self.plotMPC:
# Figure to plot MPC thoughts
self.fig, self.axs = plt.subplots(self.ns+self.nu)
ylabels = ['$x$', '$\\theta$', '$\\dot{x}$', '$\\dot{\\theta}$']
for ii in range(self.ns):
self.axs[ii].set(xlabel='Time(s)',ylabel=ylabels[ii])
self.axs[ii].grid()
for ii in range(self.ns,self.ns+self.nu):
self.axs[ii].set(xlabel='Time(s)',ylabel='u')
self.axs[ii].grid()
def compute_speed_profile(self, Constraints):
"""
Compute a speed profile for the path. Assign a reference velocity
to each waypoint based on its curvature.
:param Constraints: constraints on acceleration and velocity
curvature of the path
"""
# Set optimization horizon
N = self.n_waypoints - 1
# Constraints
a_min = np.ones(N - 1) * Constraints['a_min']
a_max = np.ones(N - 1) * Constraints['a_max']
v_min = np.ones(N) * Constraints['v_min']
v_max = np.ones(N) * Constraints['v_max']
# Maximum lateral acceleration
ay_max = Constraints['ay_max']
# Inequality Matrix
D1 = np.zeros((N - 1, N))
# Iterate over horizon
for i in range(N):
# Get information about current waypoint
current_waypoint = self.get_waypoint(i)
next_waypoint = self.get_waypoint(i + 1)
# distance between waypoints
li = next_waypoint - current_waypoint
# curvature of waypoint
ki = current_waypoint.kappa
# Fill operator matrix
# dynamics of acceleration
if i < N - 1:
D1[i, i:i + 2] = np.array([-1 / (2 * li), 1 / (2 * li)])
# Compute dynamic constraint on velocity
v_max_dyn = np.sqrt(ay_max / (np.abs(ki) + self.eps))
if v_max_dyn < v_max[i]:
v_max[i] = v_max_dyn
# Construct inequality matrix
D1 = sparse.csc_matrix(D1)
D2 = sparse.eye(N)
D = sparse.vstack([D1, D2], format='csc')
# Get upper and lower bound vectors for inequality constraints
l = np.hstack([a_min, v_min])
u = np.hstack([a_max, v_max])
# Set cost matrices
P = sparse.eye(N, format='csc')
q = -1 * v_max
# Solve optimization problem
problem = osqp.OSQP()
problem.setup(P=P, q=q, A=D, l=l, u=u, verbose=False)
speed_profile = problem.solve().x
# Assign reference velocity to every waypoint
for i, wp in enumerate(self.waypoints[:-1]):
wp.v_ref = speed_profile[i]
self.waypoints[-1].v_ref = self.waypoints[-2].v_ref
def solve_via_data(self,
data,
warm_start,
verbose,
solver_opts,
solver_cache=None):
import osqp
P = data[s.P]
q = data[s.Q]
A = sp.vstack([data[s.A], data[s.F]]).tocsc()
data['full_A'] = A
uA = np.concatenate((data[s.B], data[s.G]))
data['u'] = uA
lA = np.concatenate([data[s.B], -np.inf * np.ones(data[s.G].shape)])
data['l'] = lA
if solver_cache is not None and self.name() in solver_cache:
# Use cached data.
solver, old_data, results = solver_cache[self.name()]
same_pattern = (P.shape == old_data[s.P].shape and
all(P.indptr == old_data[s.P].indptr)) and \
(A.shape == old_data['full_A'].shape and
all(A.indptr == old_data['full_A'].indptr))
else:
same_pattern = False
# If sparsity pattern differs need to do setup.
if warm_start and same_pattern:
new_args = {}
for key in ['q', 'l', 'u']:
if any(data[key] != old_data[key]):
new_args[key] = data[key]
factorizing = False
if any(P.indices != old_data[s.P].indices):
new_args['Px_idx'] = P.indices
factorizing = True
if any(P.data != old_data[s.P].data):
new_args['Px'] = P.data
factorizing = True
if any(A.indices != old_data['full_A'].indices):
new_args['Ax_idx'] = A.indices
factorizing = True
if any(A.data != old_data['full_A'].data):
new_args['Ax'] = A.data
factorizing = True
if new_args:
solver.update(**new_args)
# Map OSQP statuses back to CVXPY statuses
status = self.STATUS_MAP.get(results.info.status_val,
s.SOLVER_ERROR)
if status == s.OPTIMAL:
solver.warm_start(results.x, results.y)
# Polish if factorizing.
solver_opts['polish'] = solver_opts.get('polish', factorizing)
solver.update_settings(verbose=verbose, **solver_opts)
else:
# Initialize and solve problem
solver_opts['polish'] = solver_opts.get('polish', True)
solver = osqp.OSQP()
solver.setup(P, q, A, lA, uA, verbose=verbose, **solver_opts)
results = solver.solve()
if solver_cache is not None:
solver_cache[self.name()] = (solver, data, results)
return results
def mpc_increment(Ad_list, Bd_list, gd_list, x_tilda_vec, Xr, pred_x_tilda,
pred_del_u, Q, QN, R, N, xmin_tilda, xmax_tilda, del_umin,
del_umax):
"""
Incremental MPC
x_tilda_vec : [nx+nu, nx+nu]
Xr : [nx, N+1]
Q : [nx, nx]
R : [nu, nu]
"""
tic_mat = time.time()
# ========== Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1)) ==========
nx = Ad_list[0].shape[0]
nu = Bd_list[0].shape[1]
# Cast MPC problem to a QP:
# x = (x(0),x(1),...,x(N), u(0),...,u(N-1))
# Objective function
# C_tilda = [I, 0]
# Q_tilda = C_tilda.T * Q * C_tilta : (nx+nu, nx) * (nx, nx) * (nx, nx+nu) => (nx+nu, nx+nu)
C_tilda = sparse.hstack([sparse.eye(nx),
np.zeros([nx, nu])]) # (nx, nx+nu)
Q_tilda = C_tilda.transpose() * Q * C_tilda
Q_tilda_N = C_tilda.transpose() * QN * C_tilda
# - quadratic objective (P)
P = sparse.block_diag([
sparse.kron(sparse.eye(N), Q_tilda), # Q x (N+1) on diagonal
Q_tilda_N,
sparse.kron(sparse.eye(N), R), # R X (N) on diagonal
]).tocsc()
# - linear objective (q)
Q_C_tilda = Q * C_tilda
QN_C_tilda = QN * C_tilda
Q_C_tilda_trans = Q_C_tilda.transpose()
QN_C_tilda_trans = QN_C_tilda.transpose()
q = -Q_C_tilda_trans.dot(Xr[:, 0]) # index 0
for ii in range(N - 1):
q = np.hstack([q,
-Q_C_tilda_trans.dot(Xr[:, ii + 1])]) # index 1 ~ N-1
q = np.hstack([q, -QN_C_tilda_trans.dot(Xr[:, N]),
np.zeros(N * nu)]) # index N
# Augmentation for Incremental Control
Ad_sys = Ad_list[0]
Bd_sys = Bd_list[0]
Aug_A_sys = np.hstack([Ad_sys, Bd_sys])
Aug_A_increment = sparse.hstack(
[sparse.csr_matrix((nu, nx)),
sparse.eye(nu)])
Ad_tilda = sparse.vstack([Aug_A_sys, Aug_A_increment])
Bd_tilda = sparse.vstack([Bd_sys, sparse.eye(nu)])
Ax_Ad = sparse.csc_matrix(Ad_tilda)
Ax_diag = sparse.kron(sparse.eye(N + 1), -sparse.eye(nx + nu))
Bu_Bd = sparse.csc_matrix(Bd_tilda)
for i in range(N - 1):
Ad_sys = Ad_list[i + 1]
Bd_sys = Bd_list[i + 1]
Aug_A_sys = np.hstack([Ad_sys, Bd_sys])
Aug_A_increment = sparse.hstack(
[sparse.csr_matrix((nu, nx)),
sparse.eye(nu)])
Ad_tilda = sparse.vstack([Aug_A_sys, Aug_A_increment])
Bd_tilda = sparse.vstack([Bd_sys, sparse.eye(nu)])
Ax_Ad = sparse.block_diag([Ax_Ad, Ad_tilda])
Bu_Bd = sparse.block_diag([Bu_Bd, Bd_tilda])
Ax_Ad_top = sparse.kron(np.ones(N + 1), np.zeros((nx + nu, nx + nu)))
Ax_Ad_side = sparse.kron(np.ones((N, 1)), np.zeros((nx + nu, nx + nu)))
Ax = Ax_diag + sparse.vstack(
[Ax_Ad_top, sparse.hstack([Ax_Ad, Ax_Ad_side])])
Bu_Bd_top = sparse.kron(np.ones(N), np.zeros((nx + nu, nu)))
Bu = sparse.vstack([Bu_Bd_top, Bu_Bd])
Aeq = sparse.hstack([Ax, Bu])
# - Equality constraint (linear dynamics) : lower bound and upper bound
leq = -x_tilda_vec # later ueq == leq
for i in range(N):
gd_tilda = np.vstack([gd_list[i], np.zeros(
(nu, 1))]) # gd_tilda for augmented system
gd_tilda = np.squeeze(gd_tilda, axis=1) # from (N,1) to (N,)
leq = np.hstack([leq, -gd_tilda])
# leq = np.hstack([-x_tilda_vec, np.zeros(N*nx)])
ueq = leq
# Original Code
# ----- input and state constraints -----
Aineq = sparse.eye((N + 1) * (nx + nu) + N * nu)
lineq = np.hstack(
[np.kron(np.ones(N + 1), xmin_tilda),
np.kron(np.ones(N), del_umin)])
uineq = np.hstack(
[np.kron(np.ones(N + 1), xmax_tilda),
np.kron(np.ones(N), del_umax)])
# ----- OSQP constraints -----
A = sparse.vstack([Aeq, Aineq]).tocsc()
lb = np.hstack([leq, lineq])
ub = np.hstack([ueq, uineq])
print("matrix time :", time.time() - tic_mat)
# ==========Create an OSQP object and Setup workspace ==========
tic_solve = time.time()
prob = osqp.OSQP()
prob.setup(P, q, A, lb, ub, verbose=False, polish=False,
warm_start=False) # verbose: print output.
# Solve
res = prob.solve()
# Check solver status
if res.info.status != 'solved':
print('OSQP did not solve the problem!')
raise ValueError('OSQP did not solve the problem!')
print("solver time :", time.time() - tic_solve)
tic_pred = time.time()
# Predictive States and Actions
sol_state = res.x[:-N * nu]
sol_action = res.x[-N * nu:]
for ii in range((N + 1) * (nx + nu)):
if ii % (nx + nu) == 0:
pred_x_tilda[0, ii // (nx + nu)] = sol_state[ii] # X
elif ii % (nx + nu) == 1:
pred_x_tilda[1, ii // (nx + nu)] = sol_state[ii] # Y
elif ii % (nx + nu) == 2:
pred_x_tilda[2, ii // (nx + nu)] = sol_state[ii] # Vx
elif ii % (nx + nu) == 3:
pred_x_tilda[3, ii // (nx + nu)] = sol_state[ii] # Yaw
elif ii % (nx + nu) == 4:
pred_x_tilda[4, ii // (nx + nu)] = sol_state[ii] # Steer
else: # ii % (nx+nu) == 5:
pred_x_tilda[5, ii // (nx + nu)] = sol_state[ii] # accel_track
for jj in range((N) * nu):
if jj % nu == 0:
pred_del_u[0, jj // nu] = sol_action[jj]
else: # jj % nu == 1
pred_del_u[1, jj // nu] = sol_action[jj]
pred_del_u[:, -1] = pred_del_u[:, -2] # append last control
print("Parsing pred state action time :", time.time() - tic_pred)
return pred_x_tilda, pred_del_u
'sigma': 1e-06,
'scaled_termination': False,
'check_termination': 1,
'polish': False,
'verbose': True,
'linsys_solver': 'suitesparse ldl'
}
qp = mpbpy.QuadprogProblem(P, q, A, l, u)
res_gurobi = qp.solve(solver=mpbpy.GUROBI, verbose=True)
model = osqppurepy.OSQP()
model.setup(P=P, q=q, A=A, l=l, u=u, **osqp_opts)
res_osqppurepy = model.solve()
# Solve with SuiteSparse LDL
model = osqp.OSQP()
model.setup(P=P, q=q, A=A, l=l, u=u, **osqp_opts)
res_osqp = model.solve()
# Check difference with gurobi
if res_gurobi.status == 'optimal':
print("Difference OSQP vs Gurobi")
print(" - primal = %.4f" %
(np.linalg.norm(res_gurobi.x - res_osqp.x) /
np.linalg.norm(res_gurobi.x)))
print(" - dual = %.4f" %
(np.linalg.norm(res_gurobi.y - res_osqp.y) /
np.linalg.norm(res_gurobi.y)))
def __init__(self,
m: int,
sid: str,
params_path: str,
input_path: str,
logging_path: str = None,
ci: bool = False):
"""Class to construct QP-optimizer by a manually designed vector matrix notation.
:param m: number of velocity points
:param sid: optimized ID 'PerfSQP' or 'EmergSQP'
:param params_path: absolute path to folder containing config file .ini
:param input_path: absolute path to folder containing variable vehicle and track information
:param ci: switch to construct an object of this class used within the CI/CD jobs
:Authors:
Thomas Herrmann <*****@*****.**>
:Created on:
01.11.2019
"""
# --- Retrieve SQP settings from parameter-file
self.sqp_stgs = params_vp_sqp(m=m, sid=sid, params_path=params_path)[0]
# Store params_path
self.params_path = params_path
# number of velocity optimization variables
self.m = m
# counter for how many velocity variables one slack is valid
self.slack_every_v = self.sqp_stgs['slack_every_v']
# number of slack variables
self.n = int(np.ceil(m / self.slack_every_v))
# weight on velocity slacks
self.ev_vel_w = None
# determines whether A, P, u, l, q-matrices have been filled once
self.b_ini_done = False
# ID of optimizer object
self.sid = sid
################################################################################################################
# --- Pre-allocation of QP matrices
################################################################################################################
# --- Matrix setup for Emergency SQP and Performance SQP
if self.sid == 'EmergSQP':
# --- no F_ini-constraint in emergency profile
self.Am =\
np.empty((1 * 1 + m * 0 + (m - 1) * 6 + (m - 2) * 1 + self.n * 1, m - 1 + self.n), dtype=np.float64)
self.lo = np.empty(
(1 * 1 + m * 0 + (m - 1) * 6 + (m - 2) * 1 + self.n * 1, 1),
dtype=np.float64)
self.up = np.empty(
(1 * 1 + m * 0 + (m - 1) * 6 + (m - 2) * 1 + self.n * 1, 1),
dtype=np.float64)
else:
self.Am =\
np.empty((1 * 2 + m * 0 + (m - 1) * 5 + (m - 2) * 2 + self.n * 1, m - 1 + self.n), dtype=np.float64)
self.lo = np.empty(
(1 * 2 + m * 0 + (m - 1) * 5 + (m - 2) * 2 + self.n * 1, 1),
dtype=np.float64)
self.up = np.empty(
(1 * 2 + m * 0 + (m - 1) * 5 + (m - 2) * 2 + self.n * 1, 1),
dtype=np.float64)
print('*** --- ' + sid + ' solver initialized --- ***')
################################################################################################################
# --- Initialize Logging
################################################################################################################
if logging_path is not None:
# create logger for Performance SQP
self.logger_perf = logging.getLogger('sqp_logger_perf')
self.logger_perf.setLevel(logging.DEBUG)
os.makedirs(logging_path, exist_ok=True)
fh_perf = logging.FileHandler(
logging_path + '/sqp_perf_' +
datetime.datetime.now().strftime("%Y_%m_%d") + '_' +
datetime.datetime.now().strftime("%H_%M") + '.log')
fh_perf.setLevel(logging.DEBUG)
formatter = logging.Formatter('%(message)s')
fh_perf.setFormatter(formatter)
self.logger_perf.addHandler(fh_perf)
# create logger for Emergency SQP
self.logger_emerg = logging.getLogger('sqp_logger_emerg')
self.logger_emerg.setLevel(logging.DEBUG)
fh_emerg = logging.FileHandler(
logging_path + '/sqp_emerg_' +
datetime.datetime.now().strftime("%Y_%m_%d") + '_' +
datetime.datetime.now().strftime("%H_%M") + '.log')
fh_emerg.setLevel(logging.DEBUG)
formatter = logging.Formatter('%(message)s')
fh_emerg.setFormatter(formatter)
self.logger_emerg.addHandler(fh_emerg)
################################################################################################################
# --- Assign numeric values to hard-coded parameters in QP
################################################################################################################
# --- Performance SQP settings
# load SQP config files
sqp_config = configparser.ConfigParser()
# load QP config sparsity patterns
sqp_sparsity = configparser.ConfigParser()
if not sqp_config.read(self.params_path + 'sqp_config.ini'):
raise ValueError(
'Specified SQP config file does not exist or is empty!')
# check whether we are running in CI/CD job
if ci:
calc_sparsity(params_path=params_path,
logging_path=logging_path,
m_perf=m,
m_emerg=m)
src = logging_path + '/sparsity/.'
dst = params_path
os.system("cp -r -n " + src + " " + dst)
if not sqp_sparsity.read(self.params_path + 'sqp_sparsity_' + sid + str(m) + '.ini') and \
self.sqp_stgs['b_sparse_matrix_fill']:
raise ValueError(
'Specified SQP sparsity file does not exist or is empty!')
# --- Performance SQP settings
if self.sid == 'PerfSQP':
sym_sc_ = {
'm_t_':
sqp_config.getfloat('VEHICLE', 'mass_t'),
'Pmax_kW_':
sqp_config.getfloat('VEHICLE', 'P_max_kW'),
'Fmax_kN_':
sqp_config.getfloat('VEHICLE', 'F_max_kN'),
'Fmin_kN_':
sqp_config.getfloat('VEHICLE', 'F_min_kN'),
'axmax_mps2_':
sqp_config.getfloat('VEHICLE', 'ax_max_mps2'),
'aymax_mps2_':
sqp_config.getfloat('VEHICLE', 'ay_max_mps2'),
'dF_kN_pos_':
sqp_config.getfloat('SOLVER_GENERAL', 'dF_kN_pos'),
'dF_kN_neg_':
sqp_config.getfloat('SOLVER_GENERAL', 'dF_kN_neg'),
'Fini_tol_':
sqp_config.getfloat('SOLVER_PERFORMANCE', 'F_ini_tol'),
'c_res_':
sqp_config.getfloat('VEHICLE', 'c_res'),
'vmin_mps_':
sqp_config.getfloat('SOLVER_GENERAL', 'v_min_mps'),
's_v_t_lim_':
sqp_config.getfloat('SOLVER_PERFORMANCE',
'slack_var_tire_lim'),
's_v_t_unit_':
sqp_config.getfloat('SOLVER_GENERAL', 'slack_var_tire_unit'),
'dvdv_w_':
sqp_config.getfloat('SOLVER_PERFORMANCE', 'penalty_jerk'),
'tre_cst_w_':
sqp_config.getfloat('SOLVER_PERFORMANCE', 'w_tre_constraint'),
's_tre_w_lin_':
sqp_config.getfloat('SOLVER_PERFORMANCE',
'penalty_slack_tire_lin'),
's_tre_w_quad_':
sqp_config.getfloat('SOLVER_PERFORMANCE',
'penalty_slack_tire_quad')
}
if self.sqp_stgs['b_sparse_matrix_fill']:
self.sparsity_pat = {
'F_ini_r':
np.array(
json.loads(
sqp_sparsity.get('symqp.F_sym_ini_cst_jac_:_1:',
'r'))), # noqa: E501
'F_ini_c':
np.array(
json.loads(
sqp_sparsity.get('symqp.F_sym_ini_cst_jac_:_1:',
'c'))), # noqa: E501
'F_r':
np.array(
json.loads(
sqp_sparsity.get('symqp.F_sym_cst_jac_1:_1:',
'r'))),
'F_c':
np.array(
json.loads(
sqp_sparsity.get('symqp.F_sym_cst_jac_1:_1:',
'c'))),
'P_r':
np.array(
json.loads(
sqp_sparsity.get('symqp.P_sym_cst_jac_:_1:',
'r'))),
'P_c':
np.array(
json.loads(
sqp_sparsity.get('symqp.P_sym_cst_jac_:_1:',
'c'))),
'Tre_r':
np.array(
json.loads(
sqp_sparsity.get('symqp.Tre_sym_cst1_jac_:_1:',
'r'))), # noqa: E501
'Tre_c':
np.array(
json.loads(
sqp_sparsity.get('symqp.Tre_sym_cst1_jac_:_1:',
'c')))
}
# --- Emergency SQP settings
else:
sym_sc_ = {
'm_t_':
sqp_config.getfloat('VEHICLE', 'mass_t'),
'Pmax_kW_':
sqp_config.getfloat('VEHICLE', 'P_max_kW'),
'Fmax_kN_':
sqp_config.getfloat('VEHICLE', 'F_max_kN'),
'Fmin_kN_':
sqp_config.getfloat('VEHICLE', 'F_min_kN'),
'axmax_mps2_':
sqp_config.getfloat('VEHICLE', 'ax_max_mps2'),
'aymax_mps2_':
sqp_config.getfloat('VEHICLE', 'ay_max_mps2'),
'dF_kN_pos_':
sqp_config.getfloat('SOLVER_GENERAL', 'dF_kN_pos'),
'dF_kN_neg_':
sqp_config.getfloat('SOLVER_GENERAL', 'dF_kN_neg'),
'Fini_tol_':
sqp_config.getfloat('SOLVER_EMERGENCY', 'F_ini_tol'),
'c_res_':
sqp_config.getfloat('VEHICLE', 'c_res'),
'vmin_mps_':
sqp_config.getfloat('SOLVER_GENERAL', 'v_min_mps'),
's_v_t_lim_':
sqp_config.getfloat('SOLVER_EMERGENCY', 'slack_var_tire_lim'),
's_v_t_unit_':
sqp_config.getfloat('SOLVER_GENERAL', 'slack_var_tire_unit'),
'dvdv_w_':
sqp_config.getfloat('SOLVER_EMERGENCY', 'penalty_jerk'),
'tre_cst_w_':
sqp_config.getfloat('SOLVER_EMERGENCY', 'w_tre_constraint'),
's_tre_w_lin_':
sqp_config.getfloat('SOLVER_EMERGENCY',
'penalty_slack_tire_lin'),
's_tre_w_quad_':
sqp_config.getfloat('SOLVER_EMERGENCY',
'penalty_slack_tire_quad')
}
if self.sqp_stgs['b_sparse_matrix_fill']:
self.sparsity_pat = {
'F_r':
np.array(
json.loads(
sqp_sparsity.get('symqp.F_sym_cst_jac_:_1:',
'r'))),
'F_c':
np.array(
json.loads(
sqp_sparsity.get('symqp.F_sym_cst_jac_:_1:',
'c'))),
'P_r':
np.array(
json.loads(
sqp_sparsity.get('symqp.P_sym_cst_jac_:_1:',
'r'))),
'P_c':
np.array(
json.loads(
sqp_sparsity.get('symqp.P_sym_cst_jac_:_1:',
'c'))),
'Tre_r':
np.array(
json.loads(
sqp_sparsity.get('symqp.Tre_sym_cst1_jac_:_1:',
'r'))), # noqa: E501
'Tre_c':
np.array(
json.loads(
sqp_sparsity.get('symqp.Tre_sym_cst1_jac_:_1:',
'c')))
}
# Assign settings to SQP object
self.sym_sc_ = sym_sc_
################################################################################################################
# --- Pre-define numeric matrices
################################################################################################################
# Vector with elements 1, ..., 1: m-1 x 1
self.ones_vec_red = np.ones((m - 1, 1), dtype=np.float64)
# unity matrix last row removed: m-1 x m+n
self.E_mat_red = np.eye(m - 1, m + self.n, dtype=np.float64)
# unity matrix last 2 rows removed: m-2 x m+n
self.E_mat_redd = np.eye(m - 2, m + self.n, dtype=np.float64)
# unity matrix last row removed: m-1 x m+n; filled with power constraint
self.EP_mat_red = np.eye(m - 1, m + self.n, dtype=np.float64)
# Matrix to store inverse elements of delta t for every discretization step
self.T_mat_inv = np.zeros((m - 1, m + self.n), dtype=np.float64)
# --- Contribution of derivative of tire slack variables to tire jacobian
for j in range(self.n):
self.T_mat_inv[j * self.slack_every_v:self.slack_every_v + j * self.slack_every_v, - self.n + j] =\
- 1 * self.sym_sc_['s_v_t_unit_']
# Inverse of vehicle mass [1/t = 1/1000kg]
self.m_inv = 1 / sym_sc_['m_t_']
# Initialization of attributes
self.err = None
self.err_inf = None
self.x0 = None
self.x0_s_t = None
self.J_jac = None
self.J_Hess = None
self.F_cst = None
self.F_cst_jac = None
self.F_ini_cst = None
self.F_ini_cst_jac = None
self.v_cst_end = None
self.v_cst_end_jac = None
self.P_cst = None
self.P_cst_jac = None
self.Tre_cst1 = None
self.Tre_cst1 = None
self.Tre_cst1_jac = None
self.Tre_cst2 = None
self.Tre_cst2_jac = None
self.Tre_cst3 = None
self.Tre_cst3_jac = None
self.Tre_cst4 = None
self.Tre_cst4_jac = None
self.dF_cst = None
self.dF_cst_jac = None
self.Am_csc = None
################################################################################################################
# --- Get variable power handler class
################################################################################################################
self.vpl = VarPowerLimits(input_path=input_path)
################################################################################################################
# --- Construct QP solver object (OSQP)
################################################################################################################
self.sol_osqp = osqp.OSQP()
################################################################################################################
# --- Initialize QP solver object (OSQP)
################################################################################################################
self.osqp_init()
Ax = sparse.kron(sparse.eye(N+1),
-sparse.eye(nx)) + sparse.kron(sparse.eye(N+1, k=-1), Ad)
Bu = sparse.kron(sparse.vstack([sparse.csc_matrix((1, N)), sparse.eye(N)]),
Bd)
Aeq = sparse.hstack([Ax, Bu])
# - OSQP constraints
A = sparse.vstack([Aeq, Aineq], format='csc')
return A, lb, ub, q, P
A, lb, ub, q, P = calc_matrices(Ad, Bd, x0, xr)
# Create an OSQP object
prob = osqp.OSQP()
# Setup workspace
prob.setup(P, q, A, lb, ub, warm_start=True)
# Process observation to state
def proc_obs(obs):
pos_sp = np.array([obs[2], -obs[1]])
mb_vel = obs[6:9]
linkpos = [0.5, 0, 0, 1.0, 0, 0]
j_pos = [obs[45], -obs[46]]
j_vel = obs[47:49]
sc = obs[49:90]
angs = np.linspace(-np.pi/2, np.pi/2, 41)
sc_xs, sc_ys = np.cos(angs)*sc, np.sin(angs)*sc
def solve(self, x, x_d, mu_d, sigDelta):
sigDelta = sigDelta * self.ksig
sigDelta = np.clip(sigDelta, 0.0, self.max_var)
# sigDelta = np.ones((self.xdim/2,1)) * self.max_var # for testing
# build Q and p matrices to specify minimization expression
Q = np.diag(
np.append(
np.append(
np.ones(self.xdim / 2, dtype=np.float32) *
(self.u_cost + self.u_prev_cost), self.p1_cost),
self.p2_cost))
self.Q = sparse.csc_matrix(Q)
self.p = 2 * np.append(
np.append(-self.mu_qp_prev * self.u_prev_cost, 0), 0)
#error dynamics for clf
e = x[:-1, :] - x_d[:-1, :]
e = np.clip(e, -self.max_error, self.max_error)
eTPG = np.matmul(e.T, np.matmul(self.P, self.G))
G_dyn = np.expand_dims(np.append(np.append(eTPG, 1), 0),
0) #append 1 for clf < d
Gsig = np.matmul(self.G, sigDelta)
GssG = np.matmul(Gsig, Gsig.T)
self.trGssGP = np.trace(np.matmul(GssG, self.P))
h_dyn = -1 * (-0.5 * np.matmul(e.T, np.matmul(Q, e)) + 0.5 * np.matmul(
e.T, np.matmul(self.P, e)) / self.clf.epsilon + 0.5 * self.trGssGP)
# build constraints for barriers
N_cbf = len(self.cbf_list)
G_cbf = np.zeros((N_cbf, self.xdim / 2 + 2), dtype=np.float32)
h_cbf = np.zeros((N_cbf, 1), dtype=np.float32)
A0x_Gmud = np.matmul(self.A0, x[:-1, :]) + np.matmul(self.G, mu_d)
GssG_22 = GssG[2:, 2:]
for i, cbf in enumerate(self.cbf_list):
h_x, dB, d2B = cbf.get_B_derivatives(x)
G_cbf[i, :] = np.append(
np.append(np.einsum('ij,jk', dB, self.G), 0), 1)
trGssGd2B = np.einsum('ii', np.einsum('ij,jk', GssG_22, d2B))
h_cbf[i, :] = -1 * (np.einsum('ij,jk', dB, A0x_Gmud) -
cbf.gamma * h_x + 0.5 * trGssGd2B)
# build constraints for control limits
ginv = np.linalg.inv(self.dyn.g(x))
l_ctrl = np.matmul(ginv, mu_d - self.dyn.f(x))
A_ctrl = ginv
G_ctrl = np.zeros((self.udim * 2, self.xdim / 2 + 2), dtype=np.float32)
h_ctrl = np.zeros((self.udim * 2, 1), dtype=np.float32)
for i in range(self.udim):
G_ctrl[i * 2, :self.xdim / 2] = -A_ctrl[i, :]
h_ctrl[i * 2] = -self.u_lim[i, 0] + l_ctrl[i]
G_ctrl[i * 2 + 1, :self.xdim / 2] = A_ctrl[i, :]
h_ctrl[i * 2 + 1] = self.u_lim[i, 1] - l_ctrl[i]
# stack into one matrix and vector
G = np.concatenate((G_dyn, G_cbf, G_ctrl), axis=0)
h = np.concatenate((h_dyn, h_cbf, h_ctrl), axis=0)
self.G_csc = sparse.csc_matrix(G)
self.h = h
# dummy lower bound
l = np.ones(h.shape, dtype=np.float32) * np.inf * -1
#Solve QP
self.prob = osqp.OSQP()
exception_called = False
mu_bar = np.zeros((self.xdim + 1), dtype=np.float32)
# try:
self.prob.setup(P=self.Q,
q=self.p,
A=self.G_csc,
l=l,
u=self.h,
verbose=self.verbose)
self.res = self.prob.solve()
# except:
# exception_called = True
# else:
mu_bar = self.res.x
# if exception_called or u_bar[0] is None or np.isnan(u_bar).any():
if mu_bar[0] is None or np.isnan(mu_bar).any():
mu_bar = np.zeros((self.xdim + 1))
self.res = None
rospy.logwarn("QP failed!")
self.mu_qp_prev = np.expand_dims(mu_bar[0:self.xdim / 2], axis=0).T
self.V = self.clf.V(x, x_d)
if self.verbose:
print('z_ref: ', x.T)
print('z_des: ', x_d.T)
print('u_lim', self.u_lim)
print('V: ', self.V)
print('Q:', Q)
print('p:', np.array(self.p))
print('G_dyn:', G_dyn)
print('h_dyn:', h_dyn)
print('trGssGP', self.trGssGP)
if h_cbf.shape[0] < 10:
print('G_cbf:', G_cbf)
print('h_cbf:', h_cbf)
print('G_ctrl:', G_ctrl)
print('h_ctrl:', h_ctrl)
print('result:', mu_bar)
return self.mu_qp_prev
def step(self):
u_set = [None] * self.NV
xx_set = [None] * self.NV
QQ_set = [None] * self.NV
u0_set = [None] * self.NV
Qt_set = [None] * self.NV
x_set = [None] * self.NV
umax = np.array([self.cons.am, self.cons.rm])
# generate backup trajectories
self.xbackup = np.empty([0, (self.mpc.N + 1) * 4])
for i in range(0, self.NV):
xx_set[i] = [None] * self.m
QQ_set[i] = [None] * self.m
Qt_set[i] = [None] * self.m
if abs(self.veh_set[i].state[1] -
(1.8 + self.veh_set[i].laneidx * 3.6)) < 0.4:
if i == 0:
mindis = 1000
idx = 0
for ii in range(1, self.NV):
if self.veh_set[ii].laneidx != self.veh_set[
0].laneidx and abs(
self.veh_set[ii].state[0] -
self.veh_set[0].state[0]) < mindis:
mindis = abs(self.veh_set[ii].state[0] -
self.veh_set[0].state[0])
idx = ii
if mindis < 4:
self.veh_set[0].laneidx = self.veh_set[idx].laneidx
else:
if with_probability(0.05):
if self.veh_set[i].laneidx == 0:
self.veh_set[i].laneidx = 1
elif self.veh_set[i].laneidx == self.N_lane - 1:
self.veh_set[i].laneidx = self.N_lane - 2
else:
if with_probability(0.5):
self.veh_set[i].laneidx += 1
else:
self.veh_set[i].laneidx -= 1
x0 = self.veh_set[i].state.copy()
x0[1] = 1.8 + self.veh_set[i].laneidx * 3.6
x0[2] = self.veh_set[0].state[2] + 0.5 * (
self.veh_set[0].state[0] - self.veh_set[i].state[0])
x0[3] = 0
con = lambda x: veh_con(x, x0, umax)
u0_set[i] = con(self.veh_set[i].state)
stop_crit = lambda x, t: t > self.dt * self.mpc.N + 2
for j in range(0, self.m):
tt, xx, uu, QQ, Qt = generate_backup_traj(
self.veh_set[i].state, self.backupcons[j], stop_crit, f0,
self.dt, True)
xx_set[i][j] = xx
Qt_set[i][j] = Qt
QQ_set[i][j] = QQ
if i > 0:
self.xbackup = np.vstack(
(self.xbackup,
np.reshape(np.array(xx[0:self.mpc.N + 1]), [1, -1])))
Ydes = 1.8 + self.veh_set[0].laneidx * 3.6
vdes = v0
xRef = np.array([0, Ydes, vdes, 0])
# print(self.veh_set[1].backupidx)
self.mpc.solve(self.veh_set[0].state, self.b, self.xbackup, xRef)
u_set[0] = self.mpc.uPred[0]
self.veh_set[0].step(u_set[0])
x_set[0] = self.veh_set[0].state
## no control
# u_set[0]=np.zeros(2)
# self.veh_set[0].step(u_set[0])
# x_set[0] = self.veh_set[0].state
## backup CBF QP
for i in range(1, self.NV):
A = []
b = []
x = self.veh_set[i].state
fi, g = dubin_fg(x)
for t in range(0, len(xx_set[i][self.veh_set[i].backupidx])):
if t % 3 == 0:
xi = xx_set[i][self.veh_set[i].backupidx][t]
h, dh = X_bdry(xi, [0, lm[self.N_lane]],
self.veh_set[i].v_width)
if h < 0.5:
dhdx = np.matmul(
dh, QQ_set[i][self.veh_set[i].backupidx][t])
if norm(dhdx.dot(g)) > 1e-6:
A.append(-dhdx.dot(g))
b.append(
dhdx.dot(fi - f0) - np.matmul(
dh, Qt_set[i][self.veh_set[i].backupidx]
[t]) + self.cons.alpha * h)
for j in range(0, self.NV):
if j != i:
if t >= len(xx_set[j][self.veh_set[j].backupidx]):
xj = xx_set[j][self.veh_set[j].backupidx][
-1] + f0 * self.dt * (t - len(xx_set[j][
self.veh_set[j].backupidx]) + 1)
else:
xj = xx_set[j][self.veh_set[j].backupidx][t]
eps = 1e-6
h = veh_col(
xi, xj,
[(self.veh_set[i].v_length +
self.veh_set[j].v_length) / 2 + 1,
(self.veh_set[i].v_width +
self.veh_set[j].v_width) / 2 + 0.2])
if h < 2:
dh = np.zeros(4)
dh[0] = (veh_col(xi + [eps, 0, 0, 0], xj, [
(self.veh_set[i].v_length +
self.veh_set[j].v_length) / 2 + 1,
(self.veh_set[i].v_width +
self.veh_set[j].v_width) / 2 + 0.2
]) - h) / eps
dh[1] = (veh_col(xi + [0, eps, 0, 0], xj, [
(self.veh_set[i].v_length +
self.veh_set[j].v_length) / 2 + 1,
(self.veh_set[i].v_width +
self.veh_set[j].v_width) / 2 + 0.2
]) - h) / eps
dhdx = np.matmul(
dh, QQ_set[i][
self.veh_set[i].backupidx][t])
if norm(dhdx.dot(g)) > 1e-6:
A.append(-dhdx.dot(g))
b.append(
dhdx.dot(fi - f0) +
self.cons.alpha * h - np.matmul(
dh, Qt_set[i][self.veh_set[i].
backupidx][t]))
if A:
A = np.array(A)
A = np.append(A, -np.ones([A.shape[0], 1]), 1)
AA = np.concatenate((A, np.identity(3)))
AA = sparse.csc_matrix(AA)
ub = np.append(np.append(np.array(b), np.array(umax)), np.inf)
lb = np.append(
np.append(-np.inf * np.ones(len(b)), np.array(-umax)), 0.0)
P = np.eye(3)
P[2][2] = 0
P = sparse.csc_matrix(P)
q = np.append(-u0_set[i], 1e6)
prob = osqp.OSQP()
prob.setup(P, q, AA, lb, ub, alpha=1.0, verbose=False)
res = prob.solve()
if res.info.status_val == 1 or res.info.status_val == 2:
u_set[i] = res.x[0:2]
else:
if res.x.shape[0] > 0:
u_set[i] = res.x[0:2]
else:
u_set[i] = u0_set[i]
else:
uu = np.maximum(u0_set[i], -umax)
u_set[i] = np.minimum(uu, umax)
self.veh_set[i].step(u_set[i])
x_set[i] = self.veh_set[i].state
## update belief and replace vehicles far away from host vehicle
if self.veh_set[i].state[0] - self.veh_set[0].state[
0] > 15 or self.veh_set[i].state[0] - self.veh_set[
0].state[0] < -15:
succ = self.replace_veh(i, 0)
if not succ:
self.replace_veh(i, 2)
else:
cbfcond = np.zeros(self.m)
xdot = dubin(x_set[i], u_set[i])
hi = np.zeros(self.m)
for j in range(0, self.m):
hij = np.zeros(self.mpc.N)
dhij = np.zeros(self.mpc.N)
for tt in range(0, self.mpc.N):
hij[tt] = veh_col(xx_set[i][j][tt],self.mpc.xPred[tt][0:4],[(self.veh_set[i].v_length+self.veh_set[0].v_length)/2,\
(self.veh_set[i].v_width+self.veh_set[0].v_width)/2],self.cons.col_alpha)
dh = np.zeros(4)
dh[0] = (veh_col(xx_set[i][j][tt]+[eps,0,0,0],self.mpc.xPred[tt][0:4],[(self.veh_set[i].v_length+self.veh_set[0].v_length)/2,\
(self.veh_set[i].v_width+self.veh_set[0].v_width)/2],self.cons.col_alpha)-hij[tt])/eps
dh[0] = (veh_col(xx_set[i][j][tt]+[0,eps,0,0],self.mpc.xPred[tt][0:4],[(self.veh_set[i].v_length+self.veh_set[0].v_length)/2,\
(self.veh_set[i].v_width+self.veh_set[0].v_width)/2],self.cons.col_alpha)-hij[tt])/eps
dhij[tt] = dh @ (QQ_set[i][j][tt] @ (xdot - f0) -
Qt_set[i][j][tt])
hi[j] = np.min(hij)
cbfcond[j] = np.mean(hij + dhij)
bi = self.b[i - 1].copy()
H = backup_trans(hi, self.cons)
bi = bi @ H
for j in range(0, self.m):
bi[j] = bi[j] * backup_input_prob(cbfcond[j], self.cons)
self.b[i - 1] = bi / np.sum(bi)
## change backup
self.veh_set[i].backupidx = np.random.choice(
range(0, self.m), 1, p=H[self.veh_set[i].backupidx])[0]
if np.isnan(self.b).any():
pdb.set_trace()
return u_set, x_set, xx_set, self.mpc.xPred[1:, 0:4]
def qp_shortest_path(wx, wy):
tx, ty, tyaw, tc, csp = generate_target_course(
wx, wy) # 2m间隔的参考路径，可以计算每点的frenet坐标 法向量+切向量
# tx, ty, tyaw, tc, csp = generate_target_course(wx, wy)
# tx_ori, ty_ori, tyaw_ori, tc_ori, csp_ori = generate_target_course(wx, wy)
# osqp for nearest route
total_dis = 0.0
sampling_dis = 2.0
sampling_num = int(csp.s[-1] // sampling_dis)
E = np.zeros([2, sampling_num])
H = np.zeros([sampling_num, sampling_num])
B = np.zeros([1, sampling_num])
next_pos = csp.calc_position(total_dis)
pos_s = [next_pos]
phi_1 = np.array(csp.calc_norm(total_dis))
phi_s = [phi_1]
for i in range(sampling_num - 1):
cur_pos = next_pos
next_pos = csp.calc_position(total_dis + sampling_dis)
pos_s.append(next_pos)
delta_x = next_pos[0] - cur_pos[0]
delta_y = next_pos[1] - cur_pos[1]
E_i = np.zeros([2, sampling_num])
phi = phi_1
phi_1 = np.array(csp.calc_norm(total_dis + sampling_dis))
phi_s.append(phi_1)
phi_xi = np.array([[phi_1[0], -phi[0]]])
phi_yi = np.array([[phi_1[1], -phi[1]]])
E_i[0][i] = 1
E_i[1][i + 1] = 1
B_i = 2 * delta_x * np.dot(phi_xi, E_i) + 2 * delta_y * np.dot(
phi_yi, E_i)
H_si_t = np.dot(phi_xi.T, phi_xi) + np.dot(phi_yi.T, phi_yi)
H += np.dot(np.dot(E_i.T, H_si_t), E_i)
B += B_i
total_dis += sampling_dis
H = H * 2
P = sparse.csc_matrix(H)
q = B[0]
A = sparse.csc_matrix(np.eye(sampling_num))
l = np.array([-2.5 for i in range(sampling_num)])
l[0] = -0.1
l[-1] = -0.1
u = np.array([2.5 for i in range(sampling_num)])
u[0] = 0.1
u[-1] = 0.1
prob = osqp.OSQP()
# Setup workspace and change alpha parameter
prob.setup(P, q, A, l, u, alpha=1.0)
# Solve problem
res = prob.solve()
print(res.x)
shortest_x = []
shortest_y = []
for i in range(sampling_num):
shortest_x.append(pos_s[i][0] + res.x[i] * phi_s[i][0])
shortest_y.append(pos_s[i][1] + res.x[i] * phi_s[i][1])
tx_shortest, ty_shortest, tyaw_shortest, tc_shortest, csp_shortest = generate_target_course(
shortest_x, shortest_y)
# x,y,yaw,k,config,P,Q
return tx_shortest, ty_shortest, tyaw_shortest, tc_shortest, csp_shortest, H, q
rho_plot = np.zeros(n_rho * n_sigma)
sigma_plot = np.zeros(n_rho * n_sigma)
r_KKT_plot = np.zeros(n_rho * n_sigma)
r_res_plot = np.zeros(n_rho * n_sigma)
cond_KKT_plot = np.zeros(n_rho * n_sigma)
n_iter_plot = np.zeros(n_rho * n_sigma)
z_idx = 0
for i in tqdm(range(len(rho_vec))):
for j in range(len(sigma_vec)):
# Get ratios of norms for columns and rows
r_KKT[i, j], cond_KKT[i, j] = get_KKT_info(P_sc, A_sc, sigma_vec[j],
rho_vec[i])
# Solve problem
m = osqp.OSQP()
m.setup(P,
q,
A,
l,
u,
rho=rho_vec[i],
sigma=sigma_vec[j],
scaling=scaling,
polish=False,
verbose=False)
res = m.solve()
# Store number of iterations
n_iter[i, j] = res.info.iter
import osqp
import numpy as np
from scipy import sparse
# Define problem data
P = sparse.csc_matrix([[4, 1], [1, 2]])
q = np.array([1, 1])
A = sparse.csc_matrix([[1, 1], [1, 0], [0, 1]])
l = np.array([1, 0, 0])
u = np.array([1, 0.7, 0.7])
# Create an OSQP object
prob = osqp.OSQP()
# Setup workspace and change alpha parameter
prob.setup(P, q, A, l, u, alpha=1.0)
# Solve problem
res = prob.solve()
# ================================================== #
# ====== Create the C code ========================= #
## Create an OSQP object
m = osqp.OSQP()
## Solver initialization
###settings = [eps_abs='1.e-3']
m.setup(P, q, A, l, u, eps_abs=1.e-3)
## Generate code
m.codegen('code1', project_type='Makefile', parameters='vectors')
