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
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))

``````
``````

['APPINFO', 'APPINFOCHG', 'APPSTATUS', 'AUTO_COLD', 'BAD_CYCLES', 'BNDS_CHK', 'COLDSTART', 'CSV_READ', 'CSV_WRITE', 'CTRLMODE', 'CTRL_HOR', 'CTRL_TIME', 'CTRL_UNITS', 'CV_TYPE', 'CV_WGT_SLOPE', 'CV_WGT_START', 'CYCLECOUNT', 'DBS_LEVEL', 'DBS_READ', 'DBS_WRITE', 'DIAGLEVEL', 'EV_TYPE', 'EV_WGT_SLOPE', 'FILTER', 'FRZE_CHK', 'HIST_HOR', 'HIST_UNITS', 'ICD_CALC', 'IMODE', 'ITERATIONS', 'LINEAR', 'MAX_ITER', 'MAX_MEMORY', 'MAX_TIME', 'MEAS_CHK', 'MV_DCOST_SLOPE', 'MV_STEP_HOR', 'MV_TYPE', 'NODES', 'OBJFCNVAL', 'OTOL', 'PRED_HOR', 'PRED_TIME', 'REDUCE', 'REPLAY', 'REQCTRLMODE', 'RTOL', 'SCALING', 'SENSITIVITY', 'SEQUENTIAL', 'SOLVER', 'SOLVESTATUS', 'SOLVETIME', 'SPC_CHART', 'SPECS', 'STREAM_LEVEL', 'TIME_SHIFT', 'WEB', 'WEB_MENU', 'WEB_PLOT_FREQ', 'WEB_REFRESH', '__class__', '__delattr__', '__dict__', '__dir__', '__doc__', '__eq__', '__format__', '__ge__', '__getattr__', '__getattribute__', '__gt__', '__hash__', '__init__', '__init_subclass__', '__le__', '__lt__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_initialized', '_inout_option_list', '_input_option_list', '_output_option_list', 'getOverridesString']

``````
``````

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]:

<matplotlib.legend.Legend at 0xa186b38>

``````

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)

``````
``````

---------------------------------------------------------------------------
ValueError                                Traceback (most recent call last)
<ipython-input-13-37accb1b26f7> in <module>()
106 t = np.linspace(0,5,20)
107 u = np.zeros_like(t)
--> 108 x = vdp.integrate(vdp.x0, 0, t) * t/t[-1]
109
110 guess['state'] = x

ValueError: operands could not be broadcast together with shapes (20,2) (20,)

``````
``````

In [ ]:

``````