def __init__(self, dynamics, u_open_loop, t_open_loop):
"""__init__ Create controller
Arguments:
dynamics {dynamical system} -- dynamics for the controller
u_open_loop {numpy array [Nu,Nt]} -- open loop time series
t_open_loop {numpy array [Nt,} -- time vector
"""
Controller.__init__(self, dynamics)
self.u_open_loop = u_open_loop
self.t_open_loop = t_open_loop
self.m = u_open_loop.shape[1]
def __init__(self, bilinear_dynamics, output, lin_controller_gain):
"""Create an FBLinController object.
Policy is u = (act)^-1 * (aux), where drift and act are
components of drift vector and actuation matrix corresponding to
highest-order derivatives of each output coordinate and aux is an
auxilliary linear controller.
Inputs:
Bilinear dynamics, fb_lin_dynamics: FBLinDynamics
Auxilliary linear controller, linear_controller: LinearController
"""
Controller.__init__(self, bilinear_dynamics)
self.dynamics = bilinear_dynamics
self.output = output
self.lin_controller_gain = lin_controller_gain
self.u_prev = zeros(self.dynamics.m)
def __init__(self,
dynamics,
nom_controller,
pert_noise_var,
const_offset=0,
umin=None,
umax=None):
"""Create a PDController object.
Policy is u = -K_p * e_p - K_d * e_d, where e_p and e_d are propotional
and derivative components of error.
Inputs:
Dynamics, dynamics: Dynamics
Nominal controller, controller: Controller
"""
Controller.__init__(self, dynamics)
self.nom_controller = nom_controller
self.pert_noise_var = pert_noise_var
self.const_offset = const_offset
self.umin = umin
self.umax = umax
def __init__(self,
lifted_linear_dynamics,
N,
dt,
umin,
umax,
xmin,
xmax,
Q,
R,
QN,
q_d,
const_offset=None,
terminal_constraint=False,
add_slack=False):
"""__init__ Create an MPC controller
Arguments:
lifted_linear_dynamics {LinearLiftedDynamics} -- Lifted linear continuous-time dynamics
N {integer} -- MPC prediction horizon, 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
"""
Controller.__init__(self, lifted_linear_dynamics)
self.dynamics_object = lifted_linear_dynamics
self.dt = dt
if lifted_linear_dynamics.continuous_mdl:
Ac = lifted_linear_dynamics.A
Bc = lifted_linear_dynamics.B
[self.nx, self.nu] = Bc.shape
lin_model_d = cont2discrete(
(Ac, Bc, np.eye(self.nx), np.zeros((self.nu, 1))), dt)
self._osqp_Ad = sparse.csc_matrix(lin_model_d[0])
self._osqp_Bd = sparse.csc_matrix(lin_model_d[1])
else:
self._osqp_Ad = lifted_linear_dynamics.A
self._osqp_Bd = lifted_linear_dynamics.B
[self.nx, self.nu] = self._osqp_Bd.shape
self.C = lifted_linear_dynamics.C
self.Q = Q
self.QN = QN
self.R = R
self.N = N
self.xmin = xmin
self.xmax = xmax
self.umin = umin
self.umax = umax
if const_offset is None:
self.const_offset = np.zeros(self.nu)
else:
self.const_offset = const_offset
# Total desired path
self.q_d = q_d
self.ns = q_d.shape[0]
if self.q_d.ndim == 2:
# Add copies of the final state in the desired trajectory to enable prediction beyond trajectory horizon:
self.q_d = np.hstack([
self.q_d,
np.transpose(np.tile(self.q_d[:, -1], (self.N + 1, 1)))
])
self.terminal_constraint = terminal_constraint
self.add_slack = add_slack
# Initialize OSQP MPC Problem:
self.build_objective_()
self.build_constraints_()
self.prob = osqp.OSQP()
self.prob.setup(self._osqp_P,
self._osqp_q,
self._osqp_A,
self._osqp_l,
self._osqp_u,
warm_start=True,
verbose=False)
self._osqp_result = None
self.comp_time = []
def __init__(self,
linear_dynamics,
N,
dt,
umin,
umax,
xmin,
xmax,
Q,
R,
QN,
xr,
plotMPC=False,
plotMPC_filename="",
lifting=False,
edmd_object=None,
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 __init__(self,
linear_dynamics,
N,
dt,
umin,
umax,
xmin,
xmax,
Q,
R,
QN,
xr,
plotMPC=False,
plotMPC_filename="",
lifting=False,
edmd_object=None,
name="noname",
soft=False,
D=None):
"""__init__ [summary]
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()})
name {str} -- name for all saved files (default: {"noname"})
soft {bool} -- flag to enable soft constraints (default: {False})
D {[type]} -- cost matrix for the soft variables (default: {None})
"""
Controller.__init__(self, linear_dynamics)
# Load arguments
Ac, Bc = linear_dynamics.linear_system()
[nx, nu] = Bc.shape
ns = xr.shape[0]
#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 = 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
self.plotMPC = plotMPC
self.plotMPC_filename = plotMPC_filename
self.q_d = xr
self.Q = Q
self.R = R
self.lifting = lifting
self.nu = nu
self.nx = nx
self.ns = ns
self.soft = soft
self.comp_time = []
# 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,
])
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 = False
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 + ".pdf",
bbox_inches='tight',
format='pdf',
dpi=2400)
plt.close()
# Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
# 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])
x0aQb = self.BTQbda @ x0
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 = False
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 + ".pdf",
bbox_inches='tight',
format='pdf',
dpi=2400)
plt.close()
#plt.show()
# 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=False)
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 __init__(self,
dynamics,
N,
dt,
umin,
umax,
xmin,
xmax,
Q,
R,
QN,
xr,
solver_settings,
const_offset=None,
terminal_constraint=False,
add_slack=False,
q_slack=1e3):
"""
Initialize the nonlinear mpc class.
:param dynamics: (AffindeDynamics) dynamics object describing system dynamics
:param N: (int) Prediction horizon in number of timesteps
:param dt: (float) Time interval between time steps
:param umin: (np.array) Actuation lower bounds
:param umax: (np.array) Actuation upper bounds
:param xmin: (np.array) State lower bounds
:param xmax: (np.array) State upper bounds
:param Q: (sparse.csc_matrix) State deviation penalty matrix
:param R: (sparse.csc_matrix) Actuation penalty matrix
:param QN: (sparse.csc_matrix) Terminal state deviation penalty matrix
:param xr: (np.array) Desired state, setpoint
:param const_offset: (np.array) Constant offset of the control inputs
:param terminal_constraint: (boolean) Constrain terminal state to be xr
:param add_slack: (boolean) Add slack variables to state constraints
:param q_slack: (float) Penalty value of slack terms q||s||^2, where s are the slack variables
"""
Controller.__init__(self, dynamics)
self.dynamics_object = dynamics
self.nx = self.dynamics_object.n
self.nu = self.dynamics_object.m
self.dt = dt
if type(self.dynamics_object) == BilinearLiftedDynamics:
self.C = self.dynamics_object.C
else:
self.C = np.eye(self.nx)
self.dynamics_object.lift = lambda x, t: x
self.Q = Q
self.QN = QN
self.R = R
self.N = N
self.xmin = xmin
self.xmax = xmax
self.umin = umin
self.umax = umax
if const_offset is None:
self.const_offset = np.zeros((self.nu, 1))
else:
self.const_offset = const_offset
assert xr.ndim == 1, 'Desired trajectory not supported'
self.xr = xr
self.ns = xr.shape[0]
self.terminal_constraint = terminal_constraint
self.add_slack = add_slack
self.Q_slack = q_slack * sparse.eye(self.ns * (self.N))
self.solver_settings = solver_settings
self.embed_pkg_str = 'nmpc_' + str(self.nx) + '_' + str(
self.nu) + '_' + str(self.N)
self.prep_time = []
self.qp_time = []
self.comp_time = []
self.x_iter = []
self.u_iter = []