I want to solve a minimum fuel optimal control problem in discrete time using CVXPY. Using a zero-order hold on a continuous time, linear system, the problem can be turned into a linear program with convex control constraints. I have done the basic formulation of this problem using Matlab environments like Yalmip and CVX, but I cannot get this to work in CVXPY. The issue I am having is that even though the problem seems to compile and solve completely, the output values clearly do not satisfy the boundary conditions, and when the output is plotted, the results are empty.
I have attached the code. The problem is supposed to be minimum fuel control of the double integrator; I want a control signal in the strictly positive and negative directions, each taking values between 0 and umax.
Here's the class defining the double integrator matrices:
import numpy as np
import control as ctrl
class DoubleIntegrator:
def __init__(self, nu):
self.A = np.matrix([[0,1],[0,0]])
# Note, this matrix doesn't really do anything right now
self.C = np.matrix([[1,0],[0,1]])
try:
if nu == 1:
self.B = np.matrix([[0],[1]])
self.D = np.matrix([[0],[1]])
return
elif nu == 2:
self.B = np.matrix([[0,0],[1,-1]])
self.D = np.matrix([[0,0],[1,-1]])
return
elif nu != 1 or nu != 2:
raise InputError("ERROR: Input 'nu' must be either nu = 1 or nu = 2")
except InputError as me:
print me.message
return
def makeStateSpaceSystem(self):
self.sysc = ctrl.matlab.ss(self.A, self.B, self.C, self.D)
def makeDiscreteSystem(self, dt, method):
self.sysd = ctrl.matlab.c2d(self.sysc, dt, method)
class Error(Exception):
pass
class InputError(Error):
def __init__(self, message):
self.message = message
if __name__ == '__main__':
db = DoubleIntegrator(2)
db.makeStateSpaceSystem()
db.makeDiscreteSystem(0.1, 'zoh')
print db.sysd.A[0,1]
Here's the main code:
from DoubleIntegrator import *
import numpy as np
import cvxpy as cvx
import control as ctrl
import matplotlib.pyplot as plt
db = DoubleIntegrator(2)
db.makeStateSpaceSystem()
db.makeDiscreteSystem(0.1,'zoh')
A = db.sysd.A
B = db.sysd.B
Nsim = 20
x = cvx.Variable(2, Nsim+1)
u = cvx.Variable(2, Nsim)
x0 = [1.0,0.0]
xf = [0.0,0.0]
umax = 1.0
states = []
cost = 0
for t in range(Nsim):
cost = cost + u[0,t] + u[1,t]
constr = [x[:,t+1] == A*x[:,t] + B*u[:,t],
0 <= u[0,t], u[0,t] <= umax,
0 <= u[1,t], u[1,t] <= umax]
states.append( cvx.Problem( cvx.Minimize(cost), constr ) )
prob = sum(states)
prob.constraints += [x[0,Nsim] == xf[0], x[1,Nsim] == xf[1], x[0,0] == x0[0], x[1,0] == x0[1]]
prob.solve(verbose = True)
print u.value
print x.value
f = plt.figure()
plt.subplot(411)
plt.plot(x[0,:].value)
plt.subplot(412)
plt.plot(x[1,:].value)
plt.subplot(413)
plt.plot(u[0,:].value)
plt.subplot(414)
plt.plot(u[1,:].value)
plt.show()
Thank you very much in advance for your help!
double_integrator.py
import numpy as np
from cvxpy import Variable, sum_entries, Problem, Minimize, ECOS
import control
from time import time
# model matrices
A = np.matrix([[0, 1], [0, 0]])
B = np.matrix([[0, 0], [1, -1]])
C = np.matrix([[1, 0], [0, 1]])
D = np.matrix([[0, 0], [1, -1]])
Ts = 0.1
N = 20
sys_c = control.matlab.ss(A, B, C, D)
sys_d = control.matlab.c2d(sys_c, Ts, 'zoh')
n = A.shape[0] # number of states
m = B.shape[1] # number of inputs
# optimization variables
x = Variable(n, N+1)
u = Variable(m, N)
states = []
x0 = np.array([1, 0])
xf = np.array([0, 0])
u_max = 1
for t in xrange(N):
cost = sum_entries(u[:, t])
constr = [
x[:, t+1] == sys_d.A * x[:, t] + sys_d.B * u[:, t],
u[:, t] >= 0,
u[:, t] <= u_max
]
states.append(Problem(Minimize(cost), constr))
prob = sum(states)
prob.constraints.append(x[:, 0] == x0)
prob.constraints.append(x[:, N] == xf)
exec_start = time()
prob.solve(solver=ECOS)
exec_end = time()
print "status:", prob.status
print "value:", prob.value
print "solving time:", exec_end - exec_start, "s"
print "u[0] =", u[0, :].value
print "u[1] =", u[1, :].value
print "x[0] =", x[0, :].value
print "x[1] =", x[1, :].value
Output:
root@machine:~# python double_integrator.py
status: optimal
value: 19.9999999025
solving time: 0.067615032196 s
u[0] = [[ -4.75426733e-10 -4.55386327e-10 -4.29550365e-10 -3.95761952e-10
-3.50620396e-10 -2.88248891e-10 -1.97290675e-10 -5.21543663e-11
2.21237875e-10 9.88033157e-10 9.99999957e-01 9.99999995e-01
9.99999999e-01 1.00000000e+00 1.00000000e+00 1.00000000e+00
1.00000000e+00 1.00000000e+00 1.00000000e+00 1.00000000e+00]]
u[1] = [[ 1.00000000e+00 1.00000000e+00 1.00000000e+00 1.00000000e+00
1.00000000e+00 1.00000000e+00 1.00000000e+00 9.99999999e-01
9.99999995e-01 9.99999957e-01 9.86520079e-10 2.20404086e-10
-5.26881945e-11 -1.97648318e-10 -2.88502852e-10 -3.50811744e-10
-3.95915025e-10 -4.29680212e-10 -4.55502467e-10 -4.75535165e-10]]
x[0] = [[ 1.00000000e+00 9.95000000e-01 9.80000000e-01 9.55000000e-01
9.20000000e-01 8.75000000e-01 8.20000000e-01 7.55000000e-01
6.80000000e-01 5.95000000e-01 5.00000000e-01 4.05000000e-01
3.20000000e-01 2.45000000e-01 1.80000000e-01 1.25000000e-01
8.00000001e-02 4.50000000e-02 2.00000000e-02 5.00000001e-03
1.48064807e-12]]
x[1] = [[ -1.47624932e-12 -1.00000000e-01 -2.00000000e-01 -3.00000000e-01
-4.00000000e-01 -5.00000000e-01 -6.00000000e-01 -7.00000000e-01
-8.00000000e-01 -9.00000000e-01 -9.99999995e-01 -9.00000000e-01
-8.00000000e-01 -7.00000000e-01 -6.00000000e-01 -5.00000000e-01
-4.00000000e-01 -3.00000000e-01 -2.00000000e-01 -1.00000000e-01
-1.48504278e-12]]