Uses an interface into IPOPT to solve sub-problems in iterative convex optimization.
In [1]:
import matplotlib.pyplot as plt
import numpy as np
plt.style.use('seaborn-whitegrid')
from Utils import ipopt
from EntryGuidance import Mesh
import EntryGuidance.Convex_PS as Convex
# reload(Convex)
OCP = Convex.OCP
In [6]:
solver = ipopt.Solver()
mesh = Mesh.Mesh(t0=0, tf=10, orders=[4]*20)
N = len(mesh.times)
x0 = [3,0]
A0 = np.array([[0,1],[-1,0]])
B0 = np.array([0,1])
guess = np.array([np.linspace(x0[0], 0, N), np.zeros((N,))]).T
guess_u = np.zeros(N,)
X0 = solver.create_vars(guess)
U0 = solver.create_vars(guess_u)
Ai = [A0]*N
Bi = [B0]*N
X = mesh.chunk(X0)
U = mesh.chunk(U0)
A = mesh.chunk(Ai)
B = mesh.chunk(Bi)
# End point constraints
solver.Equation(X0[0,0]==x0[0])
solver.Equation(X0[0,1]==x0[1])
solver.Equation(X0[-1,0]==0)
solver.Equation(X0[-1,1]==0)
L = []
for a, x, b, u, d, w in zip(A,X,B,U,mesh.diffs, mesh.weights): # Iterate over mesh segments
solver.StateSpace(a, x, b, u, d) # linear dynamic constraints
# Running cost computation
lagrange = u**2
la_var = solver.model.Var(0.)
solver.Equation(la_var == w.dot(lagrange)) # The use of these intermediate variables allows the obj to be written as a small sum. This avoids the 15k character limit.
L.append(la_var)
# Path constraints:
[solver.Equation(-2.6-1.3*x1+x2 <= 0) for (x1,x2) in x]
[solver.Equation(x2 >= -2.) for (x1,x2) in x]
# [solver.Equation(ui**2 <= 9) for ui in u] # control limited to 3
solver.Obj(sum(L))
# solver.model.Solver = 0 # for optimizer comparison
solver.solve()
x_sol = solver.get_values(X0)
u_sol = solver.get_values(U0)
print(dir(solver.model.options))
In [3]:
plt.figure()
plt.plot(mesh.times, u_sol)
plt.figure()
plt.plot(mesh.times, x_sol)
plt.figure()
plt.title('Phase Portrait')
plt.plot(x_sol.T[0],x_sol.T[1], label='Trajectory')
x1 = np.linspace(-2,0)
x2 = 1.3*x1 + 2.6
plt.plot(x1,x2,'k--', label='Constraints')
x1 = np.linspace(-2,3)
plt.plot(x1, -2.*np.ones_like(x1),'k--')
plt.legend()
Out[3]:
Next step:
Rewrite (or extend) the OCP to use interior-point solver in place of the specific SOCP structured solver
In [13]:
from scipy.interpolate import interp1d
class TestClass(OCP):
""" A very basic vanderpol oscillator for testing OCP solver """
def __init__(self, mu, x0, xf, tf):
self.mu = mu
self.x0 = x0
self.xf = xf
self.tf = tf
def dyn(self, x):
# returns f,g evaluated at x (vectorized)
return np.array([x[1],-x[0] + self.mu*(1-x[0]**2)*x[1]]), np.vstack((np.zeros_like(x[0]),np.ones_like(x[0]))).squeeze()
def dynamics(self, x, t, u): # integrable function
f, g = self.dyn(x)
return f + g*u
def jac(self, x, *args):
x1, x2 = x
shape = [x.shape[0]]
shape.extend(x.shape)
A = np.zeros(shape)
B = np.vstack((np.zeros_like(x1),np.ones_like(x1))).squeeze()
A[0,1,:] = np.ones_like(x[0])
A[1,0,:] = -np.ones_like(x[0]) - 2*self.mu*x1*x2
A[1,1,:] = self.mu*(1-x[0]**2)
return np.moveaxis(A, -1, 0), np.moveaxis(B, -1, 0)
def lagrange(self, t, x, u, *args):
return np.array(u)**2
# return np.array([ self.solver.model.sqrt(ui**2) for ui in u ])
def mayer(self, *args, **kwargs):
return 0
def constraints(self, t, x, u, x_ref, u_ref):
""" Implements all constraints, including:
boundary conditions
control constraints
trust regions
"""
for xi, x0i in zip(x[0], x_ref[0]):
self.solver.Equation(xi==x0i)
for xi, xfi in zip(x[-1], self.xf):
self.solver.Equation(xi==xfi)
trust_region = 4
umax = 3
# constr = []
for ti, (xi, xr) in enumerate(zip(x, x_ref)):
self.solver.Equation( sum((xi - xr)**2) < trust_region**2 )
# constr += [cvx.abs(u[ti]) <= umax] # Control constraints
# return constr + bc
return
def plot(self, T, U, X, J, ti, tcvx):
for i, xux in enumerate(zip(T, U, X)):
t, u, xc = xux
# plt.figure(1)
# plt.plot(x[0], x[1], label=str(i))
# plt.title('State Iterations (Integration)')
plt.figure(5)
plt.plot(xc[0], xc[1], label=str(i))
plt.title('State Iterations (Discretization)')
plt.figure(2)
plt.plot(t, u, label=str(i))
plt.title('Control Iterations')
plt.figure(3)
xcvx = interp1d(T[-1], X[-1].T, kind='linear', axis=0, assume_sorted=True)(ti).T
plt.plot(X[-1][0], X[-1][1], '*-', label='Chebyshev Nodes')
plt.plot(xcvx[0], xcvx[1], 'ko', label='Mesh Points')
# plt.plot(X[-1][0], X[-1][1], label='Integration')
plt.title('Optimal Trajectory')
plt.legend()
plt.figure(4)
plt.plot(T[-1], U[-1])
plt.title('Optimal control')
plt.figure(7)
plt.semilogy(J, 'o-')
plt.ylabel('Objective Function')
plt.xlabel('Iteration')
self.mesh.plot(show=False)
for fig in [2,3,5]:
plt.figure(fig)
plt.legend()
plt.show()
vdp = TestClass(mu=0.1, x0=[2, 2], xf=[0, 0], tf=5)
guess = {}
t = np.linspace(0,5,20)
u = np.zeros_like(t)
x = vdp.integrate(vdp.x0, 0, t)
guess['state'] = x
guess['control'] = u
guess['time'] = t
guess['mesh'] = [4]*6
sol = vdp.solve(guess, max_iter=10, linesearch=False, plot=True, solver='ipopt', refine=True)
In [ ]: