def osqp_solve_qp(self, P, q, G= None, h=None, A=None, b=None, initvals=None):
"""
Solve a Quadratic Program defined as:
minimize
(1/2) * x.T * P * x + q.T * x
subject to
G * x <= h
A * x == b
using OSQP <https://github.com/oxfordcontrol/osqp>.
"""
qp_A = vstack([G, A]).tocsc()
l = -inf * ones(len(h))
qp_l = hstack([l, b])
qp_u = hstack([h, b])
self.osqp = OSQP()
self.osqp.setup(P=P, q=q, A=qp_A, l=qp_l, u=qp_u, verbose=False, polish=True)
# if self.osqp is None:
# self.osqp = OSQP()
# self.osqp.setup(P=P, q=q, A=qp_A, l=qp_l, u=qp_u, verbose=False, polish=True)
# else:
# self.osqp.update(Px=P.data,Ax=qp_A.data,q=q,l=qp_l, u=qp_u)
# self.osqp.setup(P=P, q=q, A=qp_A, l=qp_l, u=qp_u, verbose=False, polish=True)
# self.osqp.update(Ax=qp_A,Ax_idx = qp_A.indices,l=qp_l, u=qp_u)
if initvals is not None:
self.osqp.warm_start(x=initvals)
res = self.osqp.solve()
if res.info.status_val == 1:
self.feasible = 1
else:
self.feasible = 0
self.Solution = res.x
def osqp_solve_qp(self, P, q, G= None, h=None, A=None, b=None, initvals=None):
"""
Solve a Quadratic Program defined as:
minimize
(1/2) * x.T * P * x + q.T * x
subject to
G * x <= h
A * x == b
using OSQP <https://github.com/oxfordcontrol/osqp>.
"""
qp_A = vstack([G, A]).tocsc()
l = -inf * ones(len(h))
qp_l = hstack([l, b])
qp_u = hstack([h, b])
self.osqp = OSQP()
self.osqp.setup(P=P, q=q, A=qp_A, l=qp_l, u=qp_u, verbose=False, polish=True)
if initvals is not None:
self.osqp.warm_start(x=initvals)
res = self.osqp.solve()
if res.info.status_val == 1:
self.feasible = 1
else:
self.feasible = 0
print("The FTOCP is not feasible at time t = ", self.time)
self.Solution = res.x
def osqp_solve_qp(P, q, G=None, h=None, A=None, b=None, initvals=None):
"""
Solve a Quadratic Program defined as:
minimize
(1/2) * x.T * P * x + q.T * x
subject to
G * x <= h
A * x == b
using OSQP <https://github.com/oxfordcontrol/osqp>.
Parameters
----------
P : scipy.sparse.csc_matrix
Symmetric quadratic-cost matrix.
q : numpy.array
Quadratic cost vector.
G : scipy.sparse.csc_matrix
Linear inequality constraint matrix.
h : numpy.array
Linear inequality constraint vector.
A : scipy.sparse.csc_matrix, optional
Linear equality constraint matrix.
b : numpy.array, optional
Linear equality constraint vector.
initvals : numpy.array, optional
Warm-start guess vector.
Returns
-------
x : array, shape=(n,)
Solution to the QP, if found, otherwise ``None``.
Note
----
OSQP requires `P` to be symmetric, and won't check for errors otherwise.
Check out for this point if you e.g. `get nan values
<https://github.com/oxfordcontrol/osqp/issues/10>`_ in your solutions.
"""
n = q.shape[0]
l = -inf * ones(n)
if A is not None:
qp_A = vstack([G, A])
qp_l = hstack([l, b])
qp_u = hstack([h, b])
else: # no equality constraint
qp_A = G
qp_l = l
qp_u = h
osqp = OSQP()
osqp.setup(P=P, q=q, A=qp_A, l=qp_l, u=qp_u, verbose=False)
if initvals is not None:
osqp.warm_start(x=initvals)
res = osqp.solve()
if res.info.status_val != osqp.constant('OSQP_SOLVED'):
warn("OSQP exited with status '%s'" % res.info.status)
return res.x
def matrix_solutions(i):
care_num_list = list(range(care_num[i-1], care_num[i]))
diff_feature_vectors = features[col[care_num_list]] - adj_features[row[care_num_list]]
diff_feature_matrix = diff_feature_vectors.dot(diff_feature_vectors.T)
P = 2 * diff_feature_matrix
q = np.zeros(diff_feature_matrix.shape[0], dtype=np.double)
G = -np.eye(diff_feature_matrix.shape[0], dtype=np.double)
h = np.zeros(shape=(diff_feature_matrix.shape[0]), dtype=np.double)
A = np.ones(shape=(diff_feature_matrix.shape[0]), dtype=np.double)
b = diff_feature_matrix.shape[0] * nodes_alpha[row[care_num[i - 1]]]
P = sp.csc_matrix(P)
A = sp.csc_matrix(A)
l = -np.inf * np.ones(len(h))
qp_A = sp.vstack([G, A]).tocsc()
qp_l = np.hstack([l, b])
qp_u = np.hstack([h, b])
osqp = OSQP()
osqp.setup(P=P, q=q, A=qp_A, l=qp_l, u=qp_u, verbose=False, max_iter=10)
res = osqp.solve()
return i, res.x
def __solve_osqp(self,
initial_value: np.ndarray = None,
return_only_solution: bool = True):
"""return the problem through osqp
"""
try:
from osqp import OSQP
except ImportError:
raise ImportError("OSQP is not installed")
P, q = self.__get_osqp_objective_parameters()
A, l, u = self.__get_osqp_constraints_parameters()
osqp_pb = OSQP()
osqp_pb.setup(P=P, q=q, A=A, l=l, u=u, verbose=False)
osqp_pb.update_settings(eps_abs=1e-7, eps_rel=1e-7, max_iter=10000)
if initial_value is not None:
osqp_pb.warm_start(x=initial_value)
res = osqp_pb.solve()
if res.info.status != 'solved':
warnings.warn("OSQP exited with status {}".format(res.info.status))
solution = res.x.reshape(self.input_shape)
if not return_only_solution:
dual_variables = {}
for idx, _ in enumerate(self.constraints):
dual_variables[idx] = res.y[self.constraints_map[idx]
['indices']].reshape(-1, 1)
output_dict = {
'solution': solution,
'status': res.info.status,
'dual_variables': dual_variables,
}
return output_dict
else:
return solution
class FTOCP(object):
""" Finite Time Optimal Control Problem (FTOCP)
Methods:
- solve: solves the FTOCP given the initial condition x0 and terminal contraints
- buildNonlinearProgram: builds the ftocp program solved by the above solve method
- model: given x_t and u_t computes x_{t+1} = f( x_t, u_t )
"""
def __init__(self, N, A, B, Q, R, Qf, Fx, bx, Fu, bu, Ff, bf, printLevel):
# Define variables
self.printLevel = printLevel
self.A = A
self.B = B
self.N = N
self.n = A.shape[1]
self.d = B.shape[1]
self.Fx = Fx
self.bx = bx
self.Fu = Fu
self.bu = bu
self.Ff = Ff
self.bf = bf
self.Q = Q
self.Qf = Qf
self.R = R
print("Initializing FTOCP")
self.buildCost()
self.buildIneqConstr()
self.buildEqConstr()
print("Done initializing FTOCP")
self.time = 0
def solve(self, x0):
"""Computes control action
Arguments:
x0: current state
"""
# Solve QP
startTimer = datetime.datetime.now()
self.osqp_solve_qp(self.H, self.q, self.G_in,
np.add(self.w_in, np.dot(self.E_in, x0)), self.G_eq,
np.dot(self.E_eq, x0))
endTimer = datetime.datetime.now()
deltaTimer = endTimer - startTimer
self.solverTime = deltaTimer
# Unpack Solution
self.unpackSolution(x0)
self.time += 1
return self.uPred[0, :]
def unpackSolution(self, x0):
# Extract predicted state and predicted input trajectories
self.xPred = np.vstack(
(x0,
np.reshape((self.Solution[np.arange(self.n * (self.N))]),
(self.N, self.n))))
self.uPred = np.reshape(
(self.Solution[self.n * (self.N) + np.arange(self.d * self.N)]),
(self.N, self.d))
if self.printLevel >= 2:
print("Optimal State Trajectory: ")
print(self.xPred)
print("Optimal Input Trajectory: ")
print(self.uPred)
if self.printLevel >= 1:
print("Solver Time: ", self.solverTime.total_seconds(),
" seconds.")
def buildIneqConstr(self):
# Hint 1: consider building submatrices and then stack them together
# Hint 2: most likely you will need to use auxiliary variables
nbx = len(self.bx)
nbu = len(self.bu)
nbf = len(self.bf)
diagonal = [self.Fx] * (self.N - 1) + [self.Ff] + [self.Fu] * (self.N)
allButTop = linalg.block_diag(*(diagonal))
G_in = np.vstack(
[np.zeros((nbx, self.n * self.N + self.d * self.N)), allButTop])
E_in = np.zeros((nbx * self.N + nbf + nbu * self.N, self.n))
E_in[:nbx, :] = -self.Fx
w_in = np.concatenate([self.bx] * self.N + [self.bf] +
[self.bu] * self.N)
if self.printLevel >= 2:
print("G_in: ")
print(G_in)
print("E_in: ")
print(E_in)
print("w_in: ", w_in)
self.G_in = sparse.csc_matrix(G_in)
self.E_in = E_in
self.w_in = w_in.T
def buildCost(self):
# Hint: you could use the function "linalg.block_diag"
barQ = linalg.block_diag(*([self.Q] * (self.N - 1) + [self.Qf]))
barR = linalg.block_diag(*([self.R] * self.N))
H = linalg.block_diag(barQ, barR)
q = np.zeros(H.shape[0])
if self.printLevel >= 2:
print("H: ")
print(H)
print("q: ", q)
self.q = q
self.H = sparse.csc_matrix(
2 * H
) # Need to multiply by two because CVX considers 1/2 in front of quadratic cost
def buildEqConstr(self):
# Hint 1: consider building submatrices and then stack them together
# Hint 2: most likely you will need to use auxiliary variables
Apart = linalg.block_diag(*([-self.A] * (self.N + 1)))[:(-self.n),
self.n:]
Ipart = linalg.block_diag(*([np.eye(self.n)] * self.N))
xPart = Ipart + Apart
uPart = linalg.block_diag(*([-self.B] * (self.N)))
G_eq = np.hstack([xPart, uPart])
E_eq = np.zeros((self.N * self.n, self.n))
E_eq[:self.n, :] = self.A
if self.printLevel >= 2:
print("G_eq: ")
print(G_eq)
print("E_eq: ")
print(E_eq)
self.G_eq = sparse.csc_matrix(G_eq)
self.E_eq = E_eq
def osqp_solve_qp(self,
P,
q,
G=None,
h=None,
A=None,
b=None,
initvals=None):
"""
Solve a Quadratic Program defined as:
minimize
(1/2) * x.T * P * x + q.T * x
subject to
G * x <= h
A * x == b
using OSQP <https://github.com/oxfordcontrol/osqp>.
"""
qp_A = vstack([G, A]).tocsc()
l = -inf * ones(len(h))
qp_l = hstack([l, b])
qp_u = hstack([h, b])
self.osqp = OSQP()
self.osqp.setup(P=P,
q=q,
A=qp_A,
l=qp_l,
u=qp_u,
verbose=False,
polish=True)
if initvals is not None:
self.osqp.warm_start(x=initvals)
res = self.osqp.solve()
if res.info.status_val == 1:
self.feasible = 1
else:
self.feasible = 0
print("The FTOCP is not feasible at time t = ", self.time)
self.Solution = res.x
class FTOCP(object):
""" Finite Time Optimal Control Problem (FTOCP)
Methods:
- solve: solves the FTOCP given the initial condition x0 and terminal contraints
- buildNonlinearProgram: builds the ftocp program solved by the above solve method
- model: given x_t and u_t computes x_{t+1} = f( x_t, u_t )
"""
def __init__(self, N, Q, R, Qf, Fx, bx, Fu, bu, Ff, bf, dt, uGuess, goal,
printLevel):
# Define variables
self.printLevel = printLevel
self.N = N
self.n = Q.shape[1]
self.d = R.shape[1]
self.Fx = Fx
self.bx = bx
self.Fu = Fu
self.bu = bu
self.Ff = Ff
self.bf = bf
self.Q = Q
self.Qf = Qf
self.R = R
self.dt = dt
self.uGuess = uGuess
self.goal = goal
self.buildIneqConstr()
self.buildAutomaticDifferentiationTree()
self.buildCost()
self.time = 0
def simForward(self, x0, uGuess):
self.xGuess = [x0]
for i in range(0, self.N):
xt = self.xGuess[i]
ut = self.uGuess[i]
self.xGuess.append(np.array(self.dynamics(xt, ut)))
def solve(self, x0):
"""Computes control action
Arguments:
x0: current state
"""
startTimer = datetime.datetime.now()
self.simForward(x0, self.uGuess)
self.buildEqConstr()
endTimer = datetime.datetime.now()
deltaTimer = endTimer - startTimer
self.linearizationTime = deltaTimer
# Solve QP
startTimer = datetime.datetime.now()
self.osqp_solve_qp(self.H, self.q, self.G_in,
np.add(self.w_in, np.dot(self.E_in, x0)), self.G_eq,
np.add(np.dot(self.E_eq, x0), self.C_eq))
endTimer = datetime.datetime.now()
deltaTimer = endTimer - startTimer
self.solverTime = deltaTimer
# Unpack Solution
self.unpackSolution(x0)
self.time += 1
return self.uPred[0, :]
def uGuessUpdate(self):
uPred = self.uPred
for i in range(0, self.N - 1):
self.uGuess[i] = self.uPred[i]
self.uGuess[-1] = self.uPred[-2]
# uPred = self.uPred
# print(uPred.shape)
# for i in range(0, self.N-1):
# self.uGuess[i] = np.array([0, 0])
# self.uGuess[-1] = np.array([0, 0])
def unpackSolution(self, x0):
# Extract predicted state and predicted input trajectories
self.xPred = np.vstack(
(x0,
np.reshape((self.Solution[np.arange(self.n * (self.N))]),
(self.N, self.n))))
self.uPred = np.reshape(
(self.Solution[self.n * (self.N) + np.arange(self.d * self.N)]),
(self.N, self.d))
if self.printLevel >= 2:
print("Predicted State Trajectory: ")
print(self.xPred)
print("Predicted Input Trajectory: ")
print(self.uPred)
if self.printLevel >= 1:
print("Linearization + buildEqConstr() Time: ",
self.linearizationTime.total_seconds(), " seconds.")
print("Solver Time: ", self.solverTime.total_seconds(),
" seconds.")
def buildIneqConstr(self):
# The inequality constraint is Gin z<= win + Ein x0
rep_a = [self.Fx] * (self.N - 1)
Mat = linalg.block_diag(linalg.block_diag(*rep_a), self.Ff)
Fxtot = np.vstack((np.zeros((self.Fx.shape[0], self.n * self.N)), Mat))
bxtot = np.append(np.tile(np.squeeze(self.bx), self.N), self.bf)
rep_b = [self.Fu] * (self.N)
Futot = linalg.block_diag(*rep_b)
butot = np.tile(np.squeeze(self.bu), self.N)
G_in = linalg.block_diag(Fxtot, Futot)
E_in = np.zeros((G_in.shape[0], self.n))
E_in[0:self.Fx.shape[0], 0:self.n] = -self.Fx
w_in = np.hstack((bxtot, butot))
if self.printLevel >= 2:
print("G_in: ")
print(G_in)
print("E_in: ")
print(E_in)
print("w_in: ", w_in)
self.G_in = sparse.csc_matrix(G_in)
self.E_in = E_in
self.w_in = w_in.T
def buildCost(self):
listQ = [self.Q] * (self.N - 1)
barQ = linalg.block_diag(linalg.block_diag(*listQ), self.Qf)
listTotR = [self.R] * (self.N)
barR = linalg.block_diag(*listTotR)
H = linalg.block_diag(barQ, barR)
goal = self.goal
# Hint: First construct a vector z_{goal} using the goal state and then leverage the matrix H
z_goal = -2 * np.concatenate([self.goal.reshape(-1)] * self.N)
z_goal = np.concatenate([z_goal, np.zeros(self.d * self.N)])
q = z_goal @ H
if self.printLevel >= 2:
print("H: ")
print(H)
print("q: ", q)
self.q = q
self.H = sparse.csc_matrix(
2 * H
) # Need to multiply by two because CVX considers 1/2 in front of quadratic cost
def buildEqConstr(self):
# Hint 1: The equality constraint is: [Gx, Gu]*z = E * x(t) + C
# Hint 2: Write on paper the matrices Gx and Gu to see how these matrices are constructed
Gx = np.eye(self.n * self.N)
Gu = np.zeros((self.n * self.N, self.d * self.N))
self.C = []
E_eq = np.zeros((Gx.shape[0], self.n))
for k in range(0, self.N):
A, B, C = self.buildLinearizedMatrices(self.xGuess[k],
self.uGuess[k])
if k == 0:
E_eq[0:self.n, :] = A
else:
Gx[k * self.n:(k + 1) * self.n,
(k - 1) * self.n:k * self.n] = -A
Gu[k * self.n:(k + 1) * self.n, k * self.d:(k + 1) * self.d] = -B
self.C = np.append(self.C, C)
G_eq = np.hstack((Gx, Gu))
C_eq = self.C
if self.printLevel >= 2:
print("G_eq: ")
print(G_eq)
print("E_eq: ")
print(E_eq)
print("C_eq: ", C_eq)
self.C_eq = C_eq
self.G_eq = sparse.csc_matrix(G_eq)
self.E_eq = E_eq
def buildAutomaticDifferentiationTree(self):
# Define variables
n = self.n
d = self.d
X = SX.sym('X', n)
U = SX.sym('U', d)
X_next = self.dynamics(X, U)
self.constraint = []
for i in range(0, n):
self.constraint = vertcat(self.constraint, X_next[i])
self.A_Eval = Function('A', [X, U], [jacobian(self.constraint, X)])
self.B_Eval = Function('B', [X, U], [jacobian(self.constraint, U)])
self.f_Eval = Function('f', [X, U], [self.constraint])
def buildLinearizedMatrices(self, x, u):
# Give a linearization point (x, u) this function return an affine approximation of the nonlinear system dynamics
A_linearized = np.array(self.A_Eval(x, u))
B_linearized = np.array(self.B_Eval(x, u))
C_linearized = np.squeeze(np.array(self.f_Eval(x, u))) - np.dot(
A_linearized, x) - np.dot(B_linearized, u)
if self.printLevel >= 3:
print("Linearization x: ", x)
print("Linearization u: ", u)
print("Linearized A")
print(A_linearized)
print("Linearized B")
print(B_linearized)
print("Linearized C")
print(C_linearized)
return A_linearized, B_linearized, C_linearized
def osqp_solve_qp(self,
P,
q,
G=None,
h=None,
A=None,
b=None,
initvals=None):
"""
Solve a Quadratic Program defined as:
minimize
(1/2) * x.T * P * x + q.T * x
subject to
G * x <= h
A * x == b
using OSQP <https://github.com/oxfordcontrol/osqp>.
"""
qp_A = vstack([G, A]).tocsc()
l = -inf * ones(len(h))
qp_l = hstack([l, b])
qp_u = hstack([h, b])
self.osqp = OSQP()
self.osqp.setup(P=P,
q=q,
A=qp_A,
l=qp_l,
u=qp_u,
verbose=False,
polish=True)
if initvals is not None:
self.osqp.warm_start(x=initvals)
res = self.osqp.solve()
if res.info.status_val == 1:
self.feasible = 1
else:
self.feasible = 0
print("The FTOCP is not feasible at time t = ", self.time)
self.Solution = res.x
def dynamics(self, x, u):
# state x = [x,y, vx, vy]
x_next = x[0] + self.dt * cos(x[3]) * x[2]
y_next = x[1] + self.dt * sin(x[3]) * x[2]
v_next = x[2] + self.dt * u[0]
theta_next = x[3] + self.dt * u[1]
state_next = [x_next, y_next, v_next, theta_next]
return state_next
def osqp_solve_qp(P, q, G=None, h=None, A=None, b=None, initvals=None):
# EA: P represents the quadratic weight composed by N times Q and R matrices.
"""
Solve a Quadratic Program defined as:
minimize
(1/2) * x.T * P * x + q.T * x
subject to
G * x <= h
A * x == b
using OSQP <https://github.com/oxfordcontrol/osqp>.
Parameters
----------
P : scipy.sparse.csc_matrix Symmetric quadratic-cost matrix.
q : numpy.array Quadratic cost vector.
G : scipy.sparse.csc_matrix Linear inequality constraint matrix.
h : numpy.array Linear inequality constraint vector.
A : scipy.sparse.csc_matrix, optional Linear equality constraint matrix.
b : numpy.array, optional Linear equality constraint vector.
initvals : numpy.array, optional Warm-start guess vector.
Returns
-------
x : array, shape=(n,)
Solution to the QP, if found, otherwise ``None``.
Note
----
OSQP requires `P` to be symmetric, and won't check for errors otherwise.
Check out for this point if you e.g. `get nan values
<https://github.com/oxfordcontrol/osqp/issues/10>`_ in your solutions.
"""
osqp = OSQP()
if G is not None:
l = -inf * ones(len(h))
if A is not None:
qp_A = vstack([G, A]).tocsc()
qp_l = hstack([l, b])
qp_u = hstack([h, b])
else: # no equality constraint
qp_A = G
qp_l = l
qp_u = h
osqp.setup(P=P,
q=q,
A=qp_A,
l=qp_l,
u=qp_u,
verbose=False,
polish=True)
else:
osqp.setup(P=P, q=q, A=None, l=None, u=None, verbose=True)
if initvals is not None:
osqp.warm_start(x=initvals)
res = osqp.solve()
if res.info.status_val != osqp.constant('OSQP_SOLVED'):
print("OSQP exited with status '%s'" % res.info.status)
feasible = 0
if res.info.status_val == osqp.constant(
'OSQP_SOLVED') or res.info.status_val == osqp.constant(
'OSQP_SOLVED_INACCURATE'
) or res.info.status_val == osqp.constant('OSQP_MAX_ITER_REACHED'):
feasible = 1
return res, feasible
class MPC():
"""Model Predicitve Controller class
Methods (needed by user):
solve: given system's state xt compute control action at
Arguments:
mpcParameters: model paramters
"""
def __init__(self, mpcParameters, predictiveModel=[]):
"""Initialization
Arguments:
mpcParameters: struct containing MPC parameters
"""
self.N = mpcParameters.N
self.Qslack = mpcParameters.Qslack
self.Q = mpcParameters.Q
self.Qf = mpcParameters.Qf
self.R = mpcParameters.R
self.dR = mpcParameters.dR
self.n = mpcParameters.n
self.d = mpcParameters.d
self.A = mpcParameters.A
self.B = mpcParameters.B
self.Fx = mpcParameters.Fx
self.Fu = mpcParameters.Fu
self.bx = mpcParameters.bx
self.bu = mpcParameters.bu
self.xRef = mpcParameters.xRef
self.slacks = mpcParameters.slacks
self.timeVarying = mpcParameters.timeVarying
self.predictiveModel = predictiveModel
if self.timeVarying == True:
self.xLin = self.predictiveModel.xStored[-1][0:self.N + 1, :]
self.uLin = self.predictiveModel.uStored[-1][0:self.N, :]
self.computeLTVdynamics()
self.OldInput = np.zeros((1, 2)) # TO DO fix size
# Build matrices for inequality constraints
self.buildIneqConstr()
self.buildCost()
self.buildEqConstr()
self.xPred = []
# initialize time
startTimer = datetime.datetime.now()
endTimer = datetime.datetime.now()
deltaTimer = endTimer - startTimer
self.solverTime = deltaTimer
self.linearizationTime = deltaTimer
self.timeStep = 0
def solve(self, x0):
"""Computes control action
Arguments:
x0: current state
"""
# If LTV active --> identify system model
if self.timeVarying == True:
self.computeLTVdynamics()
self.buildCost()
self.buildEqConstr()
self.addTerminalComponents(x0)
# Solve QP
startTimer = datetime.datetime.now()
self.osqp_solve_qp(self.H_FTOCP, self.q_FTOCP, self.F_FTOCP,
self.b_FTOCP, self.G_FTOCP,
np.add(np.dot(self.E_FTOCP, x0), self.L_FTOCP))
self.unpackSolution()
endTimer = datetime.datetime.now()
deltaTimer = endTimer - startTimer
self.solverTime = deltaTimer
# If LTV active --> compute state-input linearization trajectory
self.feasibleStateInput()
if self.timeVarying == True:
self.xLin = np.vstack((self.xPred[1:, :], self.zt))
self.uLin = np.vstack((self.uPred[1:, :], self.zt_u))
# update applied input
self.OldInput = self.uPred[0, :]
self.timeStep += 1
def computeLTVdynamics(self):
# Estimate system dynamics
self.A = []
self.B = []
self.C = []
for i in range(0, self.N):
Ai, Bi, Ci = self.predictiveModel.regressionAndLinearization(
self.xLin[i], self.uLin[i])
self.A.append(Ai)
self.B.append(Bi)
self.C.append(Ci)
def addTerminalComponents(self, x0):
# TO DO: ....
self.H_FTOCP = sparse.csc_matrix(self.H)
self.q_FTOCP = self.q
self.F_FTOCP = sparse.csc_matrix(self.F)
self.b_FTOCP = self.b
self.G_FTOCP = sparse.csc_matrix(self.G)
self.E_FTOCP = self.E
self.L_FTOCP = self.L
def feasibleStateInput(self):
self.zt = self.xPred[-1, :]
self.zt_u = self.uPred[-1, :]
def unpackSolution(self):
# Extract predicted state and predicted input trajectories
self.xPred = np.squeeze(
np.transpose(
np.reshape((self.Solution[np.arange(self.n * (self.N + 1))]),
(self.N + 1, self.n)))).T
self.uPred = np.squeeze(
np.transpose(
np.reshape(
(self.Solution[self.n *
(self.N + 1) + np.arange(self.d * self.N)]),
(self.N, self.d)))).T
def buildIneqConstr(self):
# The inequality constraint is Fz<=b
# Let's start by computing the submatrix of F relates with the state
rep_a = [self.Fx] * (self.N)
Mat = linalg.block_diag(*rep_a)
NoTerminalConstr = np.zeros(
(np.shape(Mat)[0], self.n)
) # The last state is unconstrained. There is a specific function add the terminal constraints (so that more complicated terminal constrains can be handled)
Fxtot = np.hstack((Mat, NoTerminalConstr))
bxtot = np.tile(np.squeeze(self.bx), self.N)
# Let's start by computing the submatrix of F relates with the input
rep_b = [self.Fu] * (self.N)
Futot = linalg.block_diag(*rep_b)
butot = np.tile(np.squeeze(self.bu), self.N)
# Let's stack all together
F_hard = linalg.block_diag(Fxtot, Futot)
# Add slack if need
if self.slacks == True:
nc_x = self.Fx.shape[0] # add slack only for state constraints
# Fist add add slack to existing constraints
addSlack = np.zeros((F_hard.shape[0], nc_x * self.N))
addSlack[0:nc_x * (self.N),
0:nc_x * (self.N)] = -np.eye(nc_x * (self.N))
# Now constraint slacks >= 0
I = -np.eye(nc_x * self.N)
Zeros = np.zeros((nc_x * self.N, F_hard.shape[1]))
Positivity = np.hstack((Zeros, I))
# Let's stack all together
self.F = np.vstack((np.hstack((F_hard, addSlack)), Positivity))
self.b = np.hstack((bxtot, butot, np.zeros(nc_x * self.N)))
else:
self.F = F_hard
self.b = np.hstack((bxtot, butot))
def buildEqConstr(self):
# Buil matrices for optimization (Convention from Chapter 15.2 Borrelli, Bemporad and Morari MPC book)
# The equality constraint is: G*z = E * x(t) + L
Gx = np.eye(self.n * (self.N + 1))
Gu = np.zeros((self.n * (self.N + 1), self.d * (self.N)))
E = np.zeros((self.n * (self.N + 1), self.n))
E[np.arange(self.n)] = np.eye(self.n)
L = np.zeros(self.n * (self.N + 1))
for i in range(0, self.N):
if self.timeVarying == True:
Gx[(self.n + i * self.n):(self.n + i * self.n + self.n),
(i * self.n):(i * self.n + self.n)] = -self.A[i]
Gu[(self.n + i * self.n):(self.n + i * self.n + self.n),
(i * self.d):(i * self.d + self.d)] = -self.B[i]
L[(self.n + i * self.n):(self.n + i * self.n +
self.n)] = self.C[i]
else:
Gx[(self.n + i * self.n):(self.n + i * self.n + self.n),
(i * self.n):(i * self.n + self.n)] = -self.A
Gu[(self.n + i * self.n):(self.n + i * self.n + self.n),
(i * self.d):(i * self.d + self.d)] = -self.B
if self.slacks == True:
self.G = np.hstack(
(Gx, Gu, np.zeros((Gx.shape[0], self.Fx.shape[0] * self.N))))
else:
self.G = np.hstack((Gx, Gu))
self.E = E
self.L = L
def buildCost(self):
# The cost is: (1/2) * z' H z + q' z
listQ = [self.Q] * (self.N)
Hx = linalg.block_diag(*listQ)
listTotR = [self.R + 2 * np.diag(self.dR)] * (
self.N) # Need to add dR for the derivative input cost
Hu = linalg.block_diag(*listTotR)
# Need to condider that the last input appears just once in the difference
for i in range(0, self.d):
Hu[i - self.d,
i - self.d] = Hu[i - self.d, i - self.d] - self.dR[i]
# Derivative Input Cost
OffDiaf = -np.tile(self.dR, self.N - 1)
np.fill_diagonal(Hu[self.d:], OffDiaf)
np.fill_diagonal(Hu[:, self.d:], OffDiaf)
# Cost linear term for state and input
q = -2 * np.dot(
np.append(np.tile(self.xRef, self.N + 1),
np.zeros(self.R.shape[0] * self.N)),
linalg.block_diag(Hx, self.Qf, Hu))
# Derivative Input (need to consider input at previous time step)
q[self.n * (self.N + 1):self.n * (self.N + 1) +
self.d] = -2 * np.dot(self.OldInput, np.diag(self.dR))
if self.slacks == True:
quadSlack = self.Qslack[0] * np.eye(self.Fx.shape[0] * self.N)
linSlack = self.Qslack[1] * np.ones(self.Fx.shape[0] * self.N)
self.H = linalg.block_diag(Hx, self.Qf, Hu, quadSlack)
self.q = np.append(q, linSlack)
else:
self.H = linalg.block_diag(Hx, self.Qf, Hu)
self.q = q
self.H = 2 * self.H # Need to multiply by two because CVX considers 1/2 in front of quadratic cost
def osqp_solve_qp(self,
P,
q,
G=None,
h=None,
A=None,
b=None,
initvals=None):
"""
Solve a Quadratic Program defined as:
minimize
(1/2) * x.T * P * x + q.T * x
subject to
G * x <= h
A * x == b
using OSQP <https://github.com/oxfordcontrol/osqp>.
"""
self.osqp = OSQP()
qp_A = vstack([G, A]).tocsc()
l = -inf * ones(len(h))
qp_l = hstack([l, b])
qp_u = hstack([h, b])
self.osqp.setup(P=P,
q=q,
A=qp_A,
l=qp_l,
u=qp_u,
verbose=False,
polish=True)
if initvals is not None:
self.osqp.warm_start(x=initvals)
res = self.osqp.solve()
if res.info.status_val == 1:
self.feasible = 1
else:
self.feasible = 0
self.Solution = res.x
class robustMPC():
"""Model Predicitve Controller class
Methods (needed by user):
solve: given system's state xt compute control action at
Arguments:
mpcParameters: model paramters
"""
def __init__(self, mpcParameters, predictiveModel):
"""Initialization
Arguments:
mpcParameters: struct containing MPC parameters
"""
self.N = mpcParameters.N
self.NB = mpcParameters.NB
self.Qslack = mpcParameters.Qslack
self.Q = mpcParameters.Q
self.Qf = mpcParameters.Qf
self.R = mpcParameters.R
self.dR = mpcParameters.dR
self.n = mpcParameters.n
self.d = mpcParameters.d
self.A = mpcParameters.A
self.B = mpcParameters.B
self.Fx = mpcParameters.Fx
self.Fu = mpcParameters.Fu
self.bx = mpcParameters.bx
self.bu = mpcParameters.bu
self.xRef = mpcParameters.xRef
self.m = predictiveModel.m
self.Nx = self.N * self.NB + 2
self.Nu = self.N * self.NB + 1
self.slacks = mpcParameters.slacks
self.slackdim = None
self.timeVarying = mpcParameters.timeVarying
self.predictiveModel = predictiveModel
self.osqp = None
self.BT = None
self.zpred = [None] * (self.N * self.NB + 1)
self.zcount = 0
# if self.timeVarying == True:
# self.xLin = self.predictiveModel.xStored[-1][0:self.N+1,:]
# self.uLin = self.predictiveModel.uStored[-1][0:self.N,:]
#
self.OldInput = np.zeros((1, 2)) # TO DO fix size
self.xPred = None
self.uLin = None
# initialize time
startTimer = datetime.datetime.now()
endTimer = datetime.datetime.now()
deltaTimer = endTimer - startTimer
self.solverTime = deltaTimer
self.linearizationTime = deltaTimer
self.timeStep = 0
def get_xLin(self, x, z):
if self.uLin is None:
self.uLin = np.zeros([self.Nu, self.d])
self.uLin = np.vstack((self.uLin, self.uLin[-1]))
self.xLin = np.zeros([self.Nx, self.n])
self.xLin[0] = x
for i in range(0, self.Nx - 1):
A, B, C, xp = self.predictiveModel.dyn_linearization(
self.xLin[i], self.uLin[i])
self.xLin[i + 1] = xp
def inittree(self, z):
self.BT = BranchTree(np.reshape(z, [1, self.n]), 1, 0)
q = [self.BT]
for t in range(0, len(self.zpred)):
self.zpred[i] = np.empty([0, self.n])
self.zpred[0] = np.array([z])
self.zcount = 1
while len(q) > 0:
currentbranch = q.pop(0)
if currentbranch.depth > 0:
for i in range(0, currentbranch.ztraj.shape[0]):
t = (currentbranch.depth - 1) * self.N + i
self.zpred[t] = np.append(self.zpred[t])
self.zcount += 1
if currentbranch.depth < self.NB:
zpred = self.predictiveModel.zpred_eval(
currentbranch.ztraj[-1])
p, dp = self.predictiveModel.branch_eval(
currentbranch.xtraj[-1], currentbranch.ztraj[-1])
currentbranch.p = p
currentbranch.dp = dp
for i in range(0, self.m):
newbranch = BranchTree(
zpred[:, self.n * i:self.n * (i + 1)],
p[i] * currentbranch.w, currentbranch.depth + 1)
currentbranch.addchild(newbranch)
q.append(newbranch)
def updatetree(self, z):
q = [self.BT]
self.BT.ztraj = np.reshape(z, [1, self.n])
for t in range(0, len(self.zpred)):
self.zpred[i] = np.empty([0, self.n])
self.zpred[0] = np.array([z])
while len(q) > 0:
currentbranch = q.pop(0)
if currentbranch.depth > 0:
for i in range(0, currentbranch.ztraj.shape[0]):
t = (currentbranch.depth - 1) * self.N + i
self.zpred[t] = np.append(self.zpred[t])
if currentbranch.depth < self.NB:
zpred = self.predictiveModel.zpred_eval(
currentbranch.ztraj[-1])
p, dp = self.predictiveModel.branch_eval(
currentbranch.xtraj[-1], currentbranch.ztraj[-1])
currentbranch.p = p
currentbranch.dp = dp
for i in range(0, self.m):
currentbranch.children[i].ztraj = zpred[:, self.n *
i:self.n * (i + 1)]
currentbranch.children[i].w = p[i] * currentbranch.w
q.append(children[i])
def BT2array(self):
ztraj = []
xtraj = self.xPred[1:]
q = [self.BT]
while (len(q) > 0):
curr = q.pop(0)
for child in curr.children:
ztraj.append(np.vstack((curr.ztraj[-1], child.ztraj)))
q.append(child)
return xtraj, ztraj
def solve(self, x, z, xRef=None):
"""Computes control action
Arguments:
x0: current state
"""
# If LTV active --> identify system model
if not xRef is None:
self.xRef = xRef
if self.BT is None:
self.inittree(x, z)
else:
self.updatetree(x, z)
self.buildIneqConstr()
self.buildCost()
self.buildEqConstr()
self.H_FTOCP = sparse.csc_matrix(self.H)
self.q_FTOCP = self.q
self.F_FTOCP = sparse.csc_matrix(self.F)
self.b_FTOCP = self.b
self.G_FTOCP = sparse.csc_matrix(self.G)
self.E_FTOCP = self.E
self.L_FTOCP = self.L
# Solve QP
startTimer = datetime.datetime.now()
self.osqp_solve_qp(self.H_FTOCP, self.q_FTOCP, self.F_FTOCP,
self.b_FTOCP, self.G_FTOCP,
np.add(np.dot(self.E_FTOCP, xb0), self.L_FTOCP))
self.unpackSolution()
endTimer = datetime.datetime.now()
deltaTimer = endTimer - startTimer
self.solverTime = deltaTimer
self.feasibleStateInput()
# If LTV active --> compute state-input linearization trajectory
if self.timeVarying == True:
self.xLin = np.vstack((self.xPred[1:, :], self.zt))
self.uLin = np.vstack((self.uPred[1:, :], self.zt_u))
# update applied input
self.OldInput = self.uPred[0, :]
self.timeStep += 1
self.computeLTVdynamics()
def computeLTVdynamics(self):
# Estimate system dynamics
self.A = []
self.B = []
self.C = []
self.h0 = []
self.Jh = []
for i in range(0, self.Nu):
Ai, Bi, Ci, xpi = self.predictiveModel.dyn_linearization(
self.xLin[i], self.uLin[i])
self.A.append(Ai)
self.B.append(Bi)
self.C.append(Ci)
def addTerminalComponents(self, x0):
# TO DO: ....
self.H_FTOCP = sparse.csc_matrix(self.H)
self.q_FTOCP = self.q
self.F_FTOCP = sparse.csc_matrix(self.F)
self.b_FTOCP = self.b
self.G_FTOCP = sparse.csc_matrix(self.G)
self.E_FTOCP = self.E
self.L_FTOCP = self.L
def feasibleStateInput(self):
self.zt = self.xPred[-1, :]
self.zt_u = self.uPred[-1, :]
def unpackSolution(self):
# Extract predicted state and predicted input trajectories
self.xPred = np.squeeze(
np.transpose(
np.reshape((self.Solution[np.arange(self.n * self.Nx)]),
(self.Nx, self.n)))).T
self.uPred = np.squeeze(
np.transpose(
np.reshape((self.Solution[self.n * self.Nx +
np.arange(self.d * self.Nu)]),
(self.Nu, self.d)))).T
self.xLin = self.xPred
self.uLin = self.uPred
self.uLin = np.vstack((self.uLin, self.uLin[-1]))
def buildIneqConstr(self):
# The inequality constraint is Fz<=b
# Let's start by computing the submatrix of F relates with the state
rep_a = [self.Fx] * (self.Nx - 1)
Mat = linalg.block_diag(*rep_a)
NoTerminalConstr = np.zeros(
(np.shape(Mat)[0], self.n)
) # The last state is unconstrained. There is a specific function add the terminal constraints (so that more complicated terminal constrains can be handled)
Fxtot = np.hstack((Mat, NoTerminalConstr))
bxtot = np.tile(np.squeeze(self.bx), self.Nx)
Fxbackup = np.zeros([self.zcount, self.Nx * self.n])
bxbackup = np.zeros(self.zcount)
counter = 0
for i in range(0, len(self.zpred)):
for j in range(0, self.zpred[i].shape[0]):
h, dh = self.predictiveModel.col_eval(self.xLin[i],
self.zpred[i][j])
Fxbackup[counter][self.n * i:self.n * (i + 1)] = -dh
bxbackup[counter] = h
# if self.h0[i][j][k]+self.Jh[i][j][k]@self.xLin[i]>0:
# Fxbackup[counter][(i+1)*self.n:(i+2)*self.n] = -self.Jh[i+1][j][k]
# Fxbackup[counter][i*self.n:(i+1)*self.n] = self.alphad*self.Jh[i][j][k]
# bxbackup[counter] = self.h0[i+1][j][k]-self.alphad*self.h0[i][j][k]
# else:
# Fxbackup[counter][(i+1)*self.n:(i+2)*self.n] = -self.Jh[i+1][j][k]
# bxbackup[counter] = self.h0[i+1][j][k]
counter += 1
Fxtot = np.vstack((Fxtot, Fxbackup[0:counter]))
bxtot = np.append(bxtot, bxbackup[0:counter])
self.slackdim = Fxtot.shape[0]
# Let's start by computing the submatrix of F relates with the input
rep_b = [self.Fu] * (self.Nu)
Futot = linalg.block_diag(*rep_b)
butot = np.tile(np.squeeze(self.bu), self.Nu)
# Let's stack all together
F_hard = linalg.block_diag(Fxtot, Futot)
# Add slack if need
if self.slacks == True:
nc_x = Fxtot.shape[0] # add slack only for state constraints
# Fist add add slack to existing constraints
addSlack = np.zeros((F_hard.shape[0], nc_x))
addSlack[0:nc_x, 0:nc_x] = -np.eye(nc_x)
# Now constraint slacks >= 0
I = -np.eye(nc_x)
Zeros = np.zeros((nc_x, F_hard.shape[1]))
Positivity = np.hstack((Zeros, I))
# Let's stack all together
self.F = np.vstack((np.hstack((F_hard, addSlack)), Positivity))
self.b = np.hstack((bxtot, butot, np.zeros(nc_x)))
else:
self.F = F_hard
self.b = np.hstack((bxtot, butot))
def buildEqConstr(self):
# Buil matrices for optimization (Convention from Chapter 15.2 Borrelli, Bemporad and Morari MPC book)
# The equality constraint is: G*z = E * x(t) + L
Gx = np.eye(self.n * self.Nx)
Gu = np.zeros((self.n * self.Nx, self.d * self.Nu))
E = np.zeros((self.n * self.Nx, self.n))
E[np.arange(self.n)] = np.eye(self.n)
L = np.zeros(self.n * self.Nx)
for i in range(0, self.Nu):
if self.timeVarying == True:
Gx[(self.n + i * self.n):(self.n + i * self.n + self.n),
(i * self.n):(i * self.n + self.n)] = -self.A[i]
Gu[(self.n + i * self.n):(self.n + i * self.n + self.n),
(i * self.d):(i * self.d + self.d)] = -self.B[i]
L[(self.n + i * self.n):(self.n + i * self.n +
self.n)] = self.C[i]
else:
Gx[(self.n + i * self.n):(self.n + i * self.n + self.n),
(i * self.n):(i * self.n + self.n)] = -self.A
Gu[(self.n + i * self.n):(self.n + i * self.n + self.n),
(i * self.d):(i * self.d + self.d)] = -self.B
if self.slacks == True:
self.G = np.hstack((Gx, Gu, np.zeros(
(Gx.shape[0], self.slackdim))))
else:
self.G = np.hstack((Gx, Gu))
self.E = E
self.L = L
def buildCost(self):
# The cost is: (1/2) * z' H z + q' z
listQ = [self.Q] * (self.Nx - 1)
Hx = linalg.block_diag(*listQ)
listTotR = [self.R + 2 * np.diag(self.dR)] * (
self.Nu) # Need to add dR for the derivative input cost
Hu = linalg.block_diag(*listTotR)
# Need to condider that the last input appears just once in the difference
for i in range(0, self.d):
Hu[i - self.d,
i - self.d] = Hu[i - self.d, i - self.d] - self.dR[i]
# Derivative Input Cost
OffDiaf = -np.tile(self.dR, self.Nu - 1)
np.fill_diagonal(Hu[self.d:], OffDiaf)
np.fill_diagonal(Hu[:, self.d:], OffDiaf)
# Cost linear term for state and input
q = -2 * np.dot(
np.append(np.tile(self.xRef, self.Nx),
np.zeros(self.R.shape[0] * self.Nu)),
linalg.block_diag(Hx, self.Qf, Hu))
# Derivative Input (need to consider input at previous time step)
q[self.n * self.Nx:self.n * self.Nx +
self.d] = -2 * np.dot(self.OldInput, np.diag(self.dR))
if self.slacks == True:
quadSlack = self.Qslack[0] * np.eye(self.slackdim)
linSlack = self.Qslack[1] * np.ones(self.slackdim)
self.H = linalg.block_diag(Hx, self.Qf, Hu, quadSlack)
self.q = np.append(q, linSlack)
else:
self.H = linalg.block_diag(Hx, self.Qf, Hu)
self.q = q
self.H = 2 * self.H # Need to multiply by two because CVX considers 1/2 in front of quadratic cost
def osqp_solve_qp(self,
P,
q,
G=None,
h=None,
A=None,
b=None,
initvals=None):
"""
Solve a Quadratic Program defined as:
minimize
(1/2) * x.T * P * x + q.T * x
subject to
G * x <= h
A * x == b
using OSQP <https://github.com/oxfordcontrol/osqp>.
"""
qp_A = vstack([G, A]).tocsc()
l = -inf * ones(len(h))
qp_l = hstack([l, b])
qp_u = hstack([h, b])
self.osqp = OSQP()
self.osqp.setup(P=P,
q=q,
A=qp_A,
l=qp_l,
u=qp_u,
verbose=False,
polish=True)
# if self.osqp is None:
# self.osqp = OSQP()
# self.osqp.setup(P=P, q=q, A=qp_A, l=qp_l, u=qp_u, verbose=False, polish=True)
# else:
# self.osqp.update(Px=P.data,Ax=qp_A.data,q=q,l=qp_l, u=qp_u)
# self.osqp.setup(P=P, q=q, A=qp_A, l=qp_l, u=qp_u, verbose=False, polish=True)
# self.osqp.update(Ax=qp_A,Ax_idx = qp_A.indices,l=qp_l, u=qp_u)
if initvals is not None:
self.osqp.warm_start(x=initvals)
res = self.osqp.solve()
if res.info.status_val == 1:
self.feasible = 1
else:
self.feasible = 0
self.Solution = res.x
def solve(self, x0, Last_xPredicted, uPred, A_LPV, B_LPV, C_LPV, first_it,
max_ey):
"""Computes control action
Arguments:
x0: current state position
EA: Last_xPredicted: it is just used for the warm up
EA: uPred: set of last predicted control inputs used for updating matrix A LPV
EA: A_LPV, B_LPV ,C_LPV: Set of LPV matrices
"""
# pdb.set_trace()
startTimer = datetime.datetime.now()
if (first_it < 2):
self.A, self.B, self.C = _EstimateABC(self, Last_xPredicted, uPred)
else:
self.A = A_LPV
self.B = B_LPV
self.C = C_LPV
########################################################
# Equality constraint section: (Aeq and beq)
self.Aeq, self.E, self.L, self.Eu = _buildMatEqConst(self)
# NOT considering slew rate:
# beq = np.add( np.dot(self.E,x0), self.L[:,0] ) # upper and lower equality constraint
# Considering slew rate:
uOld = [self.OldSteering[0], self.OldAccelera[0]]
beq = np.add(np.dot(self.E, x0), self.L[:, 0],
np.dot(self.Eu,
uOld)) # upper and lower equality constraint
########################################################
Q = self.Q
QN = self.QN
R = self.R
L_cf = self.L_cf
Aeq = self.Aeq # Aeq
N = self.N # Hp
nx = self.nx # num. states
nu = self.nu # num. control actions
dR = self.dR
################################################################
# NEW OPTIMIZATION PROBLEM CODE:
################################################################
# # - 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), L_cf), L_cf,
# np.zeros(N*nu)])
################################################################
# NEW OPTIMIZATION PROBLEM CODE:
# - Slew rate addition (dR)
################################################################
b = [Q] * (N)
Mx = linalg.block_diag(*b)
c = [R + 2 * np.diag(dR)] * (N)
Mu = linalg.block_diag(*c)
# Need to condider that the last input appears just once in the difference
Mu[Mu.shape[0] - 1,
Mu.shape[1] - 1] = Mu[Mu.shape[0] - 1, Mu.shape[1] - 1] - dR[1]
Mu[Mu.shape[0] - 2,
Mu.shape[1] - 2] = Mu[Mu.shape[0] - 2, Mu.shape[1] - 2] - dR[0]
# Derivative Input Cost
OffDiaf = -np.tile(dR, N - 1)
np.fill_diagonal(Mu[2:], OffDiaf)
np.fill_diagonal(Mu[:, 2:], OffDiaf)
# This is without slack lane:
M0 = linalg.block_diag(Mx, Q, Mu)
q = np.hstack([np.kron(np.ones(N), L_cf), L_cf, np.zeros(N * nu)])
# Derivative Input
q[nx * (N + 1):nx * (N + 1) + 2] = -2 * np.dot(uOld, np.diag(dR))
P = sparse.csr_matrix(2 * M0)
# Inequality constraint section:
umin = np.array([-0.249, -0.7]) # delta, a
umax = np.array([+0.249, +2.0]) # delta, a
xmin = np.array([self.min_vel, -1, -2, -max_ey,
-0.8]) # [ vx vy w ey epsi ]
xmax = np.array([self.max_vel, 1, 2, max_ey, 0.8])
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)])
""" No se como introducir constraints en "du" dado que esta formulacion QP al parecer
no me lo permite ya que Aeq ha de ser del mismo tamanio que Aineq. Pero Aeq solo tiene en cuenta
las matrices A y B mientras que Aineq tendria en cuenta lineq = [xmin, umin, dumin]
Una opcion factible para introducir esto es remodelar el modelo ampliando con dos estados, las
acciones de control como ya hicimos en un trabajo anterior.
"""
# dumin = np.array([-0.1, -0.2]) # ddelta, da
# dumax = np.array([+0.1, +0.3]) # ddelta, da
# Aineq = sparse.eye((N+1)*nx + N*nu + N*nu) # x, u and du
# lineq = np.hstack([ np.kron(np.ones(N+1), xmin), np.kron(np.ones(N), umin), np.kron(np.ones(N), dumin) ])
# uineq = np.hstack([ np.kron(np.ones(N+1), xmax), np.kron(np.ones(N), umax), np.kron(np.ones(N), dumax)])
A = sparse.vstack([Aeq, Aineq]).tocsc()
l = np.hstack([beq, lineq])
u = np.hstack([beq, uineq])
osqp = OSQP()
osqp.setup(P, q, A, l, u, warm_start=True, verbose=False, polish=True)
# if initvals is not None:
# osqp.warm_start(x=initvals)
res = osqp.solve()
# if res.info.status_val != osqp.constant('OSQP_SOLVED'):
# # print("OSQP exited with status '%s'" % res.info.status)
feasible = 0
if res.info.status_val == osqp.constant(
'OSQP_SOLVED') or res.info.status_val == osqp.constant(
'OSQP_SOLVED_INACCURATE'
) or res.info.status_val == osqp.constant(
'OSQP_MAX_ITER_REACHED'):
feasible = 1
# pdb.set_trace()
################################################################
################################################################
if feasible == 0:
print 'QUIT...'
Solution = res.x
endTimer = datetime.datetime.now()
deltaTimer = endTimer - startTimer
self.solverTime = deltaTimer
self.xPred = np.squeeze(
np.transpose(
np.reshape((Solution[np.arange(nx * (N + 1))]), (N + 1, nx))))
self.uPred = np.squeeze(
np.transpose(
np.reshape((Solution[nx * (N + 1) + np.arange(nu * N)]),
(N, nu))))
self.LinPoints = np.concatenate(
(self.xPred.T[1:, :], np.array([self.xPred.T[-1, :]])), axis=0)
self.xPred = self.xPred.T
self.uPred = self.uPred.T
def osqp_solve_qp(P, q, G=None, h=None, A=None, b=None, initvals=None):
"""
Solve a Quadratic Program defined as:
minimize
(1/2) * x.T * P * x + q.T * x
subject to
G * x <= h
A * x == b
using OSQP <https://github.com/oxfordcontrol/osqp>.
Parameters
----------
P : scipy.sparse.csc_matrix
Symmetric quadratic-cost matrix.
q : numpy.array
Quadratic cost vector.
G : scipy.sparse.csc_matrix
Linear inequality constraint matrix.
h : numpy.array
Linear inequality constraint vector.
A : scipy.sparse.csc_matrix, optional
Linear equality constraint matrix.
b : numpy.array, optional
Linear equality constraint vector.
initvals : numpy.array, optional
Warm-start guess vector.
Returns
-------
x : array, shape=(n,)
Solution to the QP, if found, otherwise ``None``.
Note
----
OSQP requires `P` to be symmetric, and won't check for errors otherwise.
Check out for this point if you e.g. `get nan values
<https://github.com/oxfordcontrol/osqp/issues/10>`_ in your solutions.
"""
l = -inf * ones(len(h))
if type(P) is ndarray:
warn(conversion_warning("P"))
P = csc_matrix(P)
if A is not None:
if A.ndim == 1:
A = A.reshape((1, A.shape[0]))
qp_A = vstack([G, A]).tocsc()
qp_l = hstack([l, b])
qp_u = hstack([h, b])
else: # no equality constraint
if type(G) is ndarray:
warn(conversion_warning("G"))
G = csc_matrix(G)
qp_A = G
qp_l = l
qp_u = h
osqp = OSQP()
osqp.setup(P=P, q=q, A=qp_A, l=qp_l, u=qp_u, verbose=False)
if initvals is not None:
osqp.warm_start(x=initvals)
res = osqp.solve()
if res.info.status_val != osqp.constant('OSQP_SOLVED'):
print("OSQP exited with status '%s'" % res.info.status)
return res.x
def runOptimiser(K, u, preOptw, initialValue, optimizer, maxWeight=10000):
"""
Args:
K (double 2d array): Similarity/distance matrix
u (double array): Mean similarity of each prototype
preOptw (double): Weight vector
initialValue (double): Initialize run
optimizer (string): qpsolver ('cvxpy' or 'osqp')
maxWeight (double): Upper bound on weight
Returns:
Prototypes, weights and objective values
"""
# Standard QP:
# minimize
# (1/2) * x.T * P * x + q.T * x
# subject to
# G * x <= h
# A * x == b
# QP Solved by Protodash:
# minimize
# (1/2) * x.T * K * x + (-u).T * x
# subject to
# G * x <= h
assert (optimizer=='cvxpy' or optimizer=='osqp'), "Please set optimizer as 'cvxpy' or 'osqp'"
d = u.shape[0]
lb = np.zeros((d, 1))
ub = maxWeight * np.ones((d, 1))
# x0 = initial value, provided optimizer supports it.
x0 = np.append( preOptw, initialValue/K[d-1, d-1] )
G = np.vstack((np.identity(d), -1*np.identity(d)))
h = np.vstack((ub, -1*lb)).ravel()
# variable shapes: K = (d,d), u = (d,) G = (2d, d), h = (2d,)
if (optimizer == 'cvxpy'):
x = cp.Variable(d)
prob = cp.Problem(cp.Minimize((1/2)*cp.quad_form(x, K) + (-u).T@x), [G@x <= h])
prob.solve()
xv = x.value.reshape(-1, 1)
xreturn = x.value
elif (optimizer == 'osqp'):
Ks = sparse.csc_matrix(K)
Gs = sparse.csc_matrix(G)
l_inf = -np.inf * np.ones(len(h))
solver = OSQP()
solver.setup(P=Ks, q=-u, A=Gs, l=l_inf, u=h, eps_abs=1e-4, eps_rel=1e-4, polish= True, verbose=False)
solver.warm_start(x=x0)
res = solver.solve()
xv = res.x.reshape(-1, 1)
xreturn = res.x
# compute objective function value
P = K
q = - u.reshape(-1, 1)
obj_value = 1/2 * np.matmul(np.matmul(xv.T, P), xv) + np.matmul(q.T, xv)
return(xreturn, obj_value[0,0])
def osqp_solve_qp(P, q, G=None, h=None, A=None, b=None, initvals=None):
"""
Solve a Quadratic Program defined as:
minimize
(1/2) * x.T * P * x + q.T * x
subject to
G * x <= h
A * x == b
using OSQP <https://github.com/oxfordcontrol/osqp>.
Parameters
----------
P : scipy.sparse.csc_matrix
Symmetric quadratic-cost matrix.
q : numpy.array
Quadratic cost vector.
G : scipy.sparse.csc_matrix
Linear inequality constraint matrix.
h : numpy.array
Linear inequality constraint vector.
A : scipy.sparse.csc_matrix, optional
Linear equality constraint matrix.
b : numpy.array, optional
Linear equality constraint vector.
initvals : numpy.array, optional
Warm-start guess vector.
Returns
-------
x : array, shape=(n,)
Solution to the QP, if found, otherwise ``None``.
Note
----
OSQP requires `P` to be symmetric, and won't check for errors otherwise.
Check out for this point if you e.g. `get nan values
<https://github.com/oxfordcontrol/osqp/issues/10>`_ in your solutions.
"""
if type(P) is ndarray:
warn(conversion_warning("P"))
P = csc_matrix(P)
solver = OSQP()
if A is None and G is None:
solver.setup(P=P, q=q, verbose=False)
elif A is not None:
if type(A) is ndarray:
warn(conversion_warning("A"))
A = csc_matrix(A)
if G is None:
solver.setup(P=P, q=q, A=A, l=b, u=b, verbose=False)
else: # G is not None
l = -inf * ones(len(h))
qp_A = vstack([G, A]).tocsc()
qp_l = hstack([l, b])
qp_u = hstack([h, b])
solver.setup(P=P, q=q, A=qp_A, l=qp_l, u=qp_u, verbose=False)
else: # A is None
if type(G) is ndarray:
warn(conversion_warning("G"))
G = csc_matrix(G)
l = -inf * ones(len(h))
solver.setup(P=P, q=q, A=G, l=l, u=h, verbose=False)
if initvals is not None:
solver.warm_start(x=initvals)
res = solver.solve()
if res.info.status_val != solver.constant('OSQP_SOLVED'):
print("OSQP exited with status '%s'" % res.info.status)
return res.x
class FTOCP(object):
""" Finite Time Optimal Control Problem (FTOCP)
Methods:
- solve: solves the FTOCP given the initial condition x0 and terminal contraints
- buildNonlinearProgram: builds the ftocp program solved by the above solve method
- model: given x_t and u_t computes x_{t+1} = f( x_t, u_t )
"""
def __init__(self, N, A, B, C, Q, R, Qf, Fx, bx, Fu, bu, Ff, bf,
printLevel):
# Define variables
self.printLevel = printLevel
self.A = A
self.B = B
self.C = C
self.N = N
self.n = A[0].shape[1]
self.d = B[0].shape[1]
self.Fx = Fx
self.bx = bx
self.Fu = Fu
self.bu = bu
self.Ff = Ff
self.bf = bf
self.Q = Q
self.Qf = Qf
self.R = R
print("Initializing FTOCP")
self.buildIneqConstr()
self.buildCost()
self.buildEqConstr()
print("Done initializing FTOCP")
self.time = 0
def solve(self, x0):
"""Computes control action
Arguments:
x0: current state
"""
# Solve QP
startTimer = datetime.datetime.now()
self.osqp_solve_qp(self.H, self.q, self.G_in,
np.add(self.w_in, np.dot(self.E_in, x0)), self.G_eq,
np.dot(self.E_eq, x0) + self.C_eq)
endTimer = datetime.datetime.now()
deltaTimer = endTimer - startTimer
self.solverTime = deltaTimer
# Unpack Solution
self.unpackSolution(x0)
self.time += 1
return self.uPred[0, :]
def unpackSolution(self, x0):
# Extract predicted state and predicted input trajectories
self.xPred = np.vstack(
(x0,
np.reshape((self.Solution[np.arange(self.n * (self.N))]),
(self.N, self.n))))
self.uPred = np.reshape(
(self.Solution[self.n * (self.N) + np.arange(self.d * self.N)]),
(self.N, self.d))
if self.printLevel >= 2:
print("Optimal State Trajectory: ")
print(self.xPred)
print("Optimal Input Trajectory: ")
print(self.uPred)
if self.printLevel >= 1:
print("Solver Time: ", self.solverTime.total_seconds(),
" seconds.")
def buildIneqConstr(self):
# Hint: Are the matrices G_in, E_in and w_in constructed using A and B?
G1 = linalg.block_diag(*([self.Fx] * (self.N - 1)))
G2 = linalg.block_diag(*([self.Fu] * self.N))
G1 = linalg.block_diag(G1, self.Ff)
G_in = linalg.block_diag(G1, G2)
G_in = np.concatenate((np.zeros(
(self.bx.shape[0], G_in.shape[1])), G_in))
w_in = np.reshape([self.bx] * self.N, -1)
w_in = np.concatenate((w_in, self.bf))
w_in2 = np.reshape([self.bu] * self.N, -1)
w_in = np.hstack((w_in, w_in2)).T
E_in = -self.Fx.T
E_in = np.hstack(
(E_in, np.zeros((self.n, w_in.shape[0] - self.Fx.shape[0]))))
E_in = E_in.T
if self.printLevel >= 2:
print("G_in: ")
print(G_in)
print("E_in: ")
print(E_in)
print("w_in: ", w_in)
self.G_in = sparse.csc_matrix(G_in)
self.E_in = E_in
self.w_in = w_in.T
def buildCost(self):
# Hint: Are the matrices H and q constructed using A and B?
barQ = linalg.block_diag(*([self.Q] * (self.N - 1)))
barQ = linalg.block_diag(barQ, self.Qf)
barR = linalg.block_diag(*([self.R] * (self.N)))
H = linalg.block_diag(barQ, barR)
q = np.zeros(H.shape[0])
if self.printLevel >= 2:
print("H: ")
print(H)
print("q: ", q)
self.q = q
self.H = sparse.csc_matrix(
2 * H
) # Need to multiply by two because CVX considers 1/2 in front of quadratic cost
def buildEqConstr(self):
Gu = linalg.block_diag(*([-self.B[0]] * self.N))
Gx1 = linalg.block_diag(*([np.eye(self.n)] * self.N))
Gx2 = linalg.block_diag(*[-a for a in self.A[1:]])
Gx2 = np.vstack((np.zeros((self.n, self.n * (self.N - 1))), Gx2))
Gx2 = np.hstack((Gx2, np.zeros((self.n * self.N, self.n))))
G_eq = np.hstack((Gx2 + Gx1, Gu))
E_eq = self.A[0].T
E_eq = np.hstack((E_eq, np.zeros(
(self.n, self.N * self.n - self.n)))).T
C_eq = np.concatenate(self.C).T
if self.printLevel >= 2:
print("G_eq: ")
print(G_eq)
print("E_eq: ")
print(E_eq)
print("C_eq: ", C_eq)
self.C_eq = C_eq
self.G_eq = sparse.csc_matrix(G_eq)
self.E_eq = E_eq
def osqp_solve_qp(self,
P,
q,
G=None,
h=None,
A=None,
b=None,
initvals=None):
"""
Solve a Quadratic Program defined as:
minimize
(1/2) * x.T * P * x + q.T * x
subject to
G * x <= h
A * x == b
using OSQP <https://github.com/oxfordcontrol/osqp>.
"""
qp_A = vstack([G, A]).tocsc()
l = -inf * ones(len(h))
qp_l = hstack([l, b])
qp_u = hstack([h, b])
self.osqp = OSQP()
self.osqp.setup(P=P,
q=q,
A=qp_A,
l=qp_l,
u=qp_u,
verbose=False,
polish=True)
if initvals is not None:
self.osqp.warm_start(x=initvals)
res = self.osqp.solve()
if res.info.status_val == 1:
self.feasible = 1
else:
self.feasible = 0
print("The FTOCP is not feasible at time t = ", self.time)
self.Solution = res.x
def fit(self, w0=None, verbose=0):
N, U, D = self.N, self.U, self.D
if verbose > 0:
t0 = time.time()
print('\nC: {:g}, {:g}, {:g}'.format(self.C1, self.C2, self.C3))
if verbose > 0:
sys.stdout.write('Creating P ... ')
sys.stdout.flush()
P = self._create_P()
if verbose > 0:
t1 = time.time()
print('{:,} by {:,} sparse matrix created in {:.1f} seconds.'.format(P.shape[0], P.shape[1], t1 - t0))
if verbose > 0:
sys.stdout.write('Creating q ... ')
sys.stdout.flush()
q = self._create_q()
if verbose > 0:
t2 = time.time()
print('{:,} dense vector created in {:.1f} seconds.'.format(q.shape[0], t2 - t1))
if verbose > 0:
sys.stdout.write('Creating A, lb ... ')
sys.stdout.flush()
A, lb = self._create_Alb()
if verbose > 0:
t3 = time.time()
print('{:,} by {:,} sparse matrix, {:,} dense vector created in {:.1f} seconds.'.format(
A.shape[0], A.shape[1], lb.shape[0], t3 - t2))
w0 = self._init_vars()
assert w0.shape == (self.num_vars,)
if verbose > 0:
sys.stdout.write('Solving QP: {:,} variables, {:,} constraints ...\n'.format(self.num_vars, self.num_cons))
sys.stdout.flush()
# solve using OSQP
qp = OSQP()
# qp.setup(P, q, A, lb, linsys_solver='mkl pardiso', verbose=True if verbose > 0 else False)
# qp.setup(P, q, A, lb, eps_prim_inf=1e-6, eps_dual_inf=1e-6, verbose=True if verbose > 0 else False)
qp.setup(P, q, A, lb, verbose=True if verbose > 0 else False)
results = qp.solve()
w = results.x
print('\n[OSQP] %s\n' % results.info.status)
self.mu = w[:D]
self.V = w[D:(U + 1) * D].reshape(U, D)
self.W = w[(U + 1) * D:(U + N + 1) * D].reshape(N, D)
# self.xi = w[(U + N + 1) * D:(U + N + 1) * D + N]
# self.delta = w[-N:]
self.trained = True
if verbose > 0:
print('Training finished in {:.1f} seconds.'.format(time.time() - t0))
def osqp_solve_qp(P,
q,
G=None,
h=None,
A=None,
b=None,
initvals=None,
verbose=False,
eps_abs=1e-4,
eps_rel=1e-4,
polish=True):
"""
Solve a Quadratic Program defined as:
.. math::
\\begin{split}\\begin{array}{ll}
\\mbox{minimize} &
\\frac{1}{2} x^T P x + q^T x \\\\
\\mbox{subject to}
& G x \\leq h \\\\
& A x = h
\\end{array}\\end{split}
using `OSQP <https://github.com/oxfordcontrol/osqp>`_.
Parameters
----------
P : scipy.sparse.csc_matrix
Symmetric quadratic-cost matrix.
q : numpy.array
Quadratic cost vector.
G : scipy.sparse.csc_matrix
Linear inequality constraint matrix.
h : numpy.array
Linear inequality constraint vector.
A : scipy.sparse.csc_matrix, optional
Linear equality constraint matrix.
b : numpy.array, optional
Linear equality constraint vector.
initvals : numpy.array, optional
Warm-start guess vector.
verbose : bool, optional
Set to `True` to print out extra information.
eps_abs : scalar, optional
Absolute convergence tolerance of the solver. Lower values yield more
precise solutions at the cost of computation time.
eps_rel : scalar, optional
Relative convergence tolerance of the solver. Lower values yield more
precise solutions at the cost of computation time.
polish : bool, optional
Perform `polishing <https://osqp.org/docs/solver/#polishing>`_, an
additional step where the solver tries to improve the accuracy of the
solution. Default is ``True``.
Returns
-------
x : array, shape=(n,)
Solution to the QP, if found, otherwise ``None``.
Note
----
OSQP requires `P` to be symmetric, and won't check for errors otherwise.
Check out for this point if you e.g. `get nan values
<https://github.com/oxfordcontrol/osqp/issues/10>`_ in your solutions.
Note
----
As of OSQP v0.6.1, the default values for both absolute and relative
tolerances are set to ``1e-3``, which results in low solver times but
imprecise solutions compared to the other QP solvers. We lower them to
``1e-5`` so that OSQP behaves closer to the norm in terms of numerical
accuracy.
"""
if type(P) is ndarray:
warn(conversion_warning("P"))
P = csc_matrix(P)
solver = OSQP()
kwargs = {
'eps_abs': eps_abs,
'eps_rel': eps_rel,
'polish': polish,
'verbose': verbose
}
if A is None and G is None:
solver.setup(P=P, q=q, **kwargs)
elif A is not None:
if type(A) is ndarray:
warn(conversion_warning("A"))
A = csc_matrix(A)
if G is None:
solver.setup(P=P, q=q, A=A, l=b, u=b, **kwargs)
else: # G is not None
l = -inf * ones(len(h))
qp_A = vstack([G, A]).tocsc()
qp_l = hstack([l, b])
qp_u = hstack([h, b])
solver.setup(P=P, q=q, A=qp_A, l=qp_l, u=qp_u, **kwargs)
else: # A is None
if type(G) is ndarray:
warn(conversion_warning("G"))
G = csc_matrix(G)
l = -inf * ones(len(h))
solver.setup(P=P, q=q, A=G, l=l, u=h, **kwargs)
if initvals is not None:
solver.warm_start(x=initvals)
res = solver.solve()
if hasattr(solver, 'constant'):
success_status = solver.constant('OSQP_SOLVED')
else: # more recent versions of OSQP
success_status = osqp.constant('OSQP_SOLVED')
if res.info.status_val != success_status:
print("OSQP exited with status '%s'" % res.info.status)
return res.x