class OcpSolver():
def __init__(self):
Td = par.Tp / par.N
# linearize model
X_lin = ca.MX.sym('X_lin', 2, 1)
U_lin = ca.MX.sym('U_lin', 1, 1)
A_c = ca.Function('A_c', \
[X_lin, U_lin], [ca.jacobian(par.ode(X_lin, U_lin), X_lin)])
A_c = A_c([0, 0], 0).full()
B_c = ca.Function('B_c', \
[X_lin, U_lin], [ca.jacobian(par.ode(X_lin, U_lin), U_lin)])
B_c = B_c([0, 0], 0).full()
# solve continuous-time Riccati equation
Qt, e, K = cn.care(A_c, B_c, par.Q, par.R)
# this is the value used in Chen1998!!
# they do not use LQR, but a modified linear controller
# Qt = np.array([[16.5926, 11.5926], [11.5926, 16.5926]])
self.integrator = Integrator(par.ode)
self.x = self.integrator.x
self.u = self.integrator.u
self.Td = Td
# start with an empty NLP
w = []
w0 = []
lbw = []
ubw = []
g = []
lbg = []
ubg = []
Xk = ca.MX.sym('X0', 2, 1)
w += [Xk]
lbw += [0, 0]
ubw += [0, 0]
w0 += [0, 0]
f = 0
# formulate the NLP
for k in range(par.N):
# new NLP variable for the control
Uk = ca.MX.sym('U_' + str(k), 1, 1)
f = f + Td*ca.mtimes(ca.mtimes(Xk.T, par.Q), Xk) \
+ Td*ca.mtimes(ca.mtimes(Uk.T, par.R), Uk)
w += [Uk]
lbw += [-par.umax]
ubw += [par.umax]
w0 += [0.0]
# integrate till the end of the interval
Xk_end = self.integrator.eval(Td, Xk, Uk)
# new NLP variable for state at end of interval
Xk = ca.MX.sym('X_' + str(k + 1), 2, 1)
w += [Xk]
lbw += [-np.inf, -np.inf]
ubw += [np.inf, np.inf]
w0 += [0, 0]
# add equality constraint
g += [Xk_end - Xk]
lbg += [0, 0]
ubg += [0, 0]
f = f + ca.mtimes(ca.mtimes(Xk_end.T, Qt), Xk_end)
g = ca.vertcat(*g)
w = ca.vertcat(*w)
# create an NLP solver
prob = {'f': f, 'x': w, 'g': g}
self.__nlp_solver = ca.nlpsol('solver', 'ipopt', prob)
self.__lbw = lbw
self.__ubw = ubw
self.__lbg = lbg
self.__ubg = ubg
self.__w0 = np.array(w0)
self.__sol_lin = np.array(w0).transpose()
self.__rti_sol = []
self.__nlp_sol = []
# create QP solver
nw = len(w0)
# define linearization point
w_lin = ca.MX.sym('w_lin', nw, 1)
w_qp = ca.MX.sym('w_qp', nw, 1)
# linearized objective = original LLS objective
f_lin = ca.substitute(
f, w, w_qp) + par.alpha * ca.dot(w_qp - w_lin, w_qp - w_lin)
nabla_g = ca.jacobian(g, w).T
g_lin = ca.substitute(g, w, w_lin) + \
ca.mtimes(ca.substitute(nabla_g, w, w_lin).T, w_qp - w_lin)
# create a QP solver
prob = {'f': f_lin, 'x': w_qp, 'g': g_lin, 'p': w_lin}
self.__rti_solver = ca.nlpsol('solver', 'ipopt', prob)
def update_x0(self, x0):
self.__lbw[0] = x0[0]
self.__lbw[1] = x0[1]
self.__ubw[0] = x0[0]
self.__ubw[1] = x0[1]
def nlp_eval(self):
sol = self.__nlp_solver(x0=self.__w0, lbx=self.__lbw, ubx=self.__ubw,\
lbg=self.__lbg, ubg=self.__ubg)
self.__status = self.__nlp_solver.stats()['return_status']
self.__nlp_sol = sol
return sol
def rti_eval(self):
# solve QP obtained linearizing the NLP at the
# current linearization point self.__rti_sol
sol = self.__rti_solver(x0=self.__w0, lbx=self.__lbw, \
ubx=self.__ubw, lbg=self.__lbg, ubg=self.__ubg,
p = self.__sol_lin)
# update current sol (i.e. linearization point)
self.__sol_lin = sol['x'].full().transpose()
self.__status = self.__rti_solver.stats()['return_status']
self.__rti_sol = sol
return sol
def get_status(self):
return self.__status
def get_rti_sol(self):
return self.__rti_sol
def get_nlp_sol(self):
return self.__nlp_sol
def set_sol_lin(self, sol_lin):
self.__sol_lin = sol_lin['x'].full()
self.__rti_sol = sol_lin
def estimate_constants(update_json=False):
# estimate L_phi_x, L_phi_u using max norm of Jacobians
# max attained at x1 = 1, x2 = -1
L_phi_u = np.sqrt((par.mu + (1 - par.mu))**2 + \
(par.mu -4*(1-par.mu))**2)
n_points = 100
u_grid = np.linspace(-par.umax, +par.umax, n_points)
norm_dphi_dx = np.zeros((n_points, 1))
for i in range(n_points):
u = u_grid[i]
dphi_dx = np.array([[u * (1 - par.mu), 1], [1, -4 * u * (1 - par.mu)]])
norm_dphi_dx[i] = np.linalg.norm(dphi_dx)
L_phi_x = np.max(norm_dphi_dx)
nsim = int(par.Tf / par.Ts)
x0 = par.x0_v[0]
n_scenarios = len(par.x0_v)
n_sqp_it = 3
# estimate a_1, a_2, a_3, sigma, mu_tilde constants based on sampling
VEXACT = np.zeros((nsim, 1, n_scenarios))
SOLEXACTNORM = np.zeros((nsim, 1, n_scenarios))
DELTAVEXACT = np.zeros((nsim - 1, 1, n_scenarios))
XNORMSQ = np.zeros((nsim, 1, n_scenarios))
XNORM = np.zeros((nsim, 1, n_scenarios))
XSIMEXACT = np.zeros((nsim + 1, 2, n_scenarios))
USIMEXACT = np.zeros((nsim, 1, n_scenarios))
exact_ocp_solver = OcpSolver()
for j in range(n_scenarios):
x0 = par.x0_v[j]
# closed loop simulation
XSIMEXACT[0, :, j] = x0
integrator = Integrator(par.ode)
exact_ocp_solver = OcpSolver()
exact_ocp_solver.nlp_eval()
for i in range(nsim):
XNORMSQ[i, 0, j] = np.linalg.norm(XSIMEXACT[i, :, j])**2
XNORM[i, 0, j] = np.linalg.norm(XSIMEXACT[i, :, j])
# update state in OCP solver
exact_ocp_solver.update_x0(XSIMEXACT[i, :, j])
solver_out = exact_ocp_solver.nlp_eval()
status = exact_ocp_solver.get_status()
if status != 'Solve_Succeeded':
raise Exception('Solver returned status {} at iteration {}'\
.format(status, i))
# get primal-dual solution
x = solver_out['x'].full()
lam_g = solver_out['lam_g'].full()
SOLEXACTNORM[i, :, j] = np.linalg.norm(np.vstack([x, lam_g]))
# get first control move
u = solver_out['x'].full()[2]
USIMEXACT[i, 0, j] = u
VEXACT[i, 0, j] = solver_out['f'].full()
XSIMEXACT[i+1,:,j] = integrator.eval(par.Ts, XSIMEXACT[i,:,j], \
u).full().transpose()
for i in range(nsim - 1):
DELTAVEXACT[i, 0, j] = VEXACT[i + 1, 0, j] - VEXACT[i, 0, j]
XSIMEXACT = XSIMEXACT[0:-1, :, :]
VEXACT_OVER_XNORMSQ = []
DELTAVEXACT_OVER_XNORMSQ = []
SOLEXACTNORM_OVER_XNORM = []
VEXACTSQRT_OVER_XNORM = []
for j in range(n_scenarios):
VEXACT_OVER_XNORMSQ.append(np.divide(VEXACT[:, 0, j], XNORMSQ[:, 0,
j]))
DELTAVEXACT_OVER_XNORMSQ.append(np.divide(DELTAVEXACT[:,0,j], \
XNORMSQ[0:-1,0,j]))
SOLEXACTNORM_OVER_XNORM.append(np.divide( \
SOLEXACTNORM[:,0,j], XNORM[:,0,j]))
VEXACTSQRT_OVER_XNORM.append(np.divide(np.sqrt(VEXACT[:,0,j]), \
XNORM[:,0,j]))
# compute constants
a1 = np.min(VEXACT_OVER_XNORMSQ)
a2 = np.max(VEXACT_OVER_XNORMSQ)
a3 = np.min(-1.0 / par.Ts * np.array(DELTAVEXACT_OVER_XNORMSQ))
sigma = np.max(SOLEXACTNORM_OVER_XNORM)
mu_tilde = np.max(VEXACTSQRT_OVER_XNORM)
print('a1 = {}, a2 = {}, a3 = {}, sigma = {}, mu_tilde = {}' \
.format(a1, a2, a3, sigma, mu_tilde))
if a1 < 0 or a2 < 0 or a3 < 0:
raise Exception('One of the a constants is \
negative (a1 = {}, a2 = {}, a3 = {})'.format(a1, a2, a3))
# estimate \hat{\kappa}
ocp_solver = OcpSolver()
lqr_solver = LqrSolver()
ocp_solver.nlp_eval()
XSIM = np.zeros((nsim + 1, 2, n_scenarios))
USIM = np.zeros((nsim, 1, n_scenarios))
VSIMSQRT = np.zeros((nsim, 1, n_scenarios))
ZSIM = np.zeros((nsim, 1, n_scenarios))
DELTAZSIM = np.zeros((nsim, 1, n_scenarios))
VSOSIM = np.zeros((nsim, 1, n_scenarios))
kappa = 0.0
for j in range(len(par.x0_v)):
x0 = par.x0_v[j]
XSIM[0, :, j] = x0
# closed loop simulation
integrator = Integrator(par.ode)
ocp_solver = OcpSolver()
ocp_solver.rti_eval()
ocp_solver.nlp_eval()
ocp_solver.update_x0(XSIM[0, :, j])
for i in range(nsim):
# update state in OCP solver
ocp_solver.update_x0(XSIM[i, :, j])
DELTAZSIM = np.zeros((n_sqp_it, 1))
if LQR_INIT:
lqr_solver.update_x0(XSIM[i, :, j])
lqr_solver.lqr_eval()
lqr_sol = lqr_solver.get_lqr_sol()
ocp_solver.set_sol_lin(lqr_sol)
for k in range(n_sqp_it):
# get primal dual solution (equalities only)
rti_sol = ocp_solver.get_rti_sol()
lam_g = rti_sol['lam_g'].full()
x = rti_sol['x'].full()
z = np.vstack((x, lam_g))
# get primal dual exact sol
exact_sol = ocp_solver.nlp_eval()
nlp_sol = ocp_solver.get_nlp_sol()
exact_lam_g = nlp_sol['lam_g'].full()
exact_x = nlp_sol['x'].full()
exact_z = np.vstack((exact_x, exact_lam_g))
DELTAZSIM[k, 0] = np.linalg.norm(z - exact_z)
# call solver
solver_out = ocp_solver.rti_eval()
status = ocp_solver.get_status()
if status != 'Solve_Succeeded':
raise Exception('Solver returned status {} at iteration {}'\
.format(status, i))
# estimate \hat{kappa}
KAPPAESTIMATE = np.zeros((n_sqp_it, 1))
for k in range(n_sqp_it - 1):
KAPPAESTIMATE[k, 0] = DELTAZSIM[k + 1, 0] / (DELTAZSIM[k, 0])
kappa = np.max([np.max(KAPPAESTIMATE), kappa])
# get first control move
u = solver_out['x'].full()[2]
USIM[i, 0, j] = u
XSIM[i+1,:,j] = integrator.eval(par.Ts, XSIM[i,:,j], \
u).full().transpose()
print('kappa = {}'.format(kappa))
if kappa > 1.0:
raise Exception('kappa bigger than 1.0!')
const = dict()
const['a1'] = a1
const['a2'] = a2
const['a3'] = a3
const['sigma'] = sigma
const['kappa_hat'] = kappa
const['mu_tilde'] = mu_tilde
const['L_phi_x'] = L_phi_x
const['L_phi_u'] = L_phi_u
if update_json:
with open('constants.json', 'w') as outfile:
json.dump(const, outfile)
return const
class LqrSolver():
def __init__(self, x_lin=[0, 0], u_lin=0):
Td = par.Tp / par.N
# linearize model
X_lin = ca.MX.sym('X_lin', 2, 1)
U_lin = ca.MX.sym('U_lin', 1, 1)
A_c = ca.Function('A_c', \
[X_lin, U_lin], [ca.jacobian(par.ode(X_lin, U_lin), X_lin)])
A_c = A_c([0, 0], 0).full()
B_c = ca.Function('B_c', \
[X_lin, U_lin], [ca.jacobian(par.ode(X_lin, U_lin), U_lin)])
B_c = B_c([0, 0], 0).full()
# solve continuous-time Riccati equation
Qt, e, K = cn.care(A_c, B_c, par.Q, par.R)
self.integrator = Integrator(par.ode)
self.x = self.integrator.x
self.u = self.integrator.u
self.Td = Td
# start with an empty NLP
w = []
w0 = []
w_lin = []
lbw = []
ubw = []
g = []
lbg = []
ubg = []
Xk = ca.MX.sym('X0', 2, 1)
w += [Xk]
lbw += [0, 0]
ubw += [0, 0]
w0 += [0, 0]
w_lin += x_lin
f = 0
# formulate the NLP
for k in range(par.N):
# new NLP variable for the control
Uk = ca.MX.sym('U_' + str(k), 1, 1)
f = f + Td*ca.mtimes(ca.mtimes(Xk.T, par.Q), Xk) \
+ Td*ca.mtimes(ca.mtimes(Uk.T, par.R), Uk)
w += [Uk]
lbw += [-np.inf]
ubw += [np.inf]
w0 += [0.0]
w_lin += [u_lin]
# integrate till the end of the interval
Xk_end = self.integrator.eval(Td, Xk, Uk)
# new NLP variable for state at end of interval
Xk = ca.MX.sym('X_' + str(k + 1), 2, 1)
w += [Xk]
lbw += [-np.inf, -np.inf]
ubw += [np.inf, np.inf]
w0 += [0, 0]
w_lin += x_lin
# add equality constraint
g += [Xk_end - Xk]
lbg += [0, 0]
ubg += [0, 0]
f = f + ca.mtimes(ca.mtimes(Xk_end.T, Qt), Xk_end)
g = ca.vertcat(*g)
w = ca.vertcat(*w)
# create an NLP solver
self.__lbw = lbw
self.__ubw = ubw
self.__lbg = lbg
self.__ubg = ubg
self.__w0 = np.array(w0)
self.__lqr_sol = []
# create QP solver
nw = len(w0)
# define linearization point
# w_lin = ca.MX.sym('w_lin', nw, 1)
w_qp = ca.MX.sym('w_qp', nw, 1)
# linearized objective = original LLS objective
f_lin = ca.substitute(f, w, w_qp)
nabla_g = ca.jacobian(g, w).T
g_lin = ca.substitute(g, w, w_lin) + \
ca.mtimes(ca.substitute(nabla_g, w, w_lin).T, w_qp - w_lin)
# create a QP solver
prob = {'f': f_lin, 'x': w_qp, 'g': g_lin}
self.__lqr_solver = ca.nlpsol('solver', 'ipopt', prob)
def update_x0(self, x0):
self.__lbw[0] = x0[0]
self.__lbw[1] = x0[1]
self.__ubw[0] = x0[0]
self.__ubw[1] = x0[1]
def lqr_eval(self):
sol = self.__lqr_solver(x0=self.__w0, lbx=self.__lbw, ubx=self.__ubw,\
lbg=self.__lbg, ubg=self.__ubg)
self.__status = self.__lqr_solver.stats()['return_status']
self.__lqr_sol = sol
return sol
def get_status(self):
return self.__status
def get_lqr_sol(self):
return self.__lqr_sol
exact_z = np.vstack((exact_x, exact_lam_g))
DELTAZSIM[i, 0, j] = np.linalg.norm(z - exact_z)
VSOSIM[i, 0, j] = VSIMSQRT[i, 0, j] + beta * DELTAZSIM[i, 0, j]
# call solver
solver_out = ocp_solver.rti_eval()
status = ocp_solver.get_status()
if status != 'Solve_Succeeded':
raise Exception('Solver returned status {} at iteration {}'\
.format(status, i))
# get first control move
u = solver_out['x'].full()[2]
USIM[i, 0, j] = u
XSIM[i+1,:,j] = integrator.eval(par.Ts, XSIM[i,:,j], \
u).full().transpose()
# get first control move (exact)
ocp_solver.update_x0(XSIMEXACT[i, :, j])
status = ocp_solver.get_status()
exact_sol = ocp_solver.nlp_eval()
if status != 'Solve_Succeeded':
raise Exception('Solver returned status {} at iteration {}'\
.format(status, i))
exact_u = exact_sol['x'].full()[2]
XSIMEXACT[i+1,:,j] = integrator.eval(par.Ts, XSIMEXACT[i,:,j], \
exact_u).full().transpose()
print(u, exact_u)
print(XSIM[i, :], XSIMEXACT[i, :])