Forming system of constraint equations using GEKKO optimization framework

Question

I'm trying to solve a simple minimum time optimal control problem using double integrator dynamics of the form,

dx1/dt = x2
dx2/dt = u

with the GEKKO optimization framework as follows:

from gekko import GEKKO
import numpy as np
import matplotlib.pyplot as plt

model = GEKKO(remote=False)

x1_initial = 0.0
x1_final = 10.0

x2_initial = 0.0
x2_final = 0.0

t_initial = 0.0
t_final = 25.0
num_timesteps = 1000
dt = (t_final - t_initial) / num_timesteps

x = model.Array(model.Var, (2, num_timesteps + 1))
u = model.Array(model.Var, num_timesteps + 1)
tf = model.Var()

for k in range(num_timesteps + 1):
    u[k].lower = -0.4
    u[k].upper = 0.4
    u[k].value = 0.0

for k in range(num_timesteps + 1):
    x[0, k].value = 5.0
    x[1, k].value = 0.0

tf.lower = t_initial
tf.upper = t_final
tf.value = t_final
dt = (tf - t_initial) / num_timesteps

def f(x, u, k):
    return np.array([x[1,k], u[k]])

for k in range(num_timesteps):
    model.Equations([x[:, k + 1] == x[:, k] + (dt/2.0)*(f(x, u, k + 1) + f(x, u, k))])
    # model.Equation(x[0, k + 1] == x[0, k] + (dt/2.0)*(x[1, k + 1] + x[1, k]))
    # model.Equation(x[1, k + 1] == x[1, k] + (dt/2.0)*(u[k + 1] + u[k]))

model.Equation(x[0, 0] == x1_initial)
model.Equation(x[0, num_timesteps] == x1_final)

model.Equation(x[1, 0] == x2_initial)
model.Equation(x[1, num_timesteps] == x2_final)

model.Minimize(tf)
model.options.solver = 3
model.solve()

# Plotting results
t = np.linspace(t_initial, tf.value, num_timesteps + 1)

u_optimal = []
for k in range(num_timesteps + 1):
    u_optimal.append(u[k].value)

x1_optimal = []
for k in range(num_timesteps + 1):
    x1_optimal.append(x[0, k].value)

x2_optimal = []
for k in range(num_timesteps + 1):
    x2_optimal.append(x[1, k].value)

plt.figure()
plt.plot(t, u_optimal)
plt.xlabel('time (s)')
plt.ylabel('u(t)')
plt.grid()

plt.figure()
plt.plot(t, x1_optimal)
plt.xlabel('time (s)')
plt.ylabel('x1(t)')
plt.grid()

plt.figure()
plt.plot(t, x2_optimal)
plt.xlabel('time (s)')
plt.ylabel('x2(t)')
plt.grid()

plt.show()

What I'm trying to do is to form a system of equality constraints using trapezoidal integration and then solve this system for the optimal control inputs using GEKKO. However, using the function definition,

def f(x, u, k):
    return np.array([x[1,k], u[k]])

in conjunction with the system of equality constraints,

for k in range(num_timesteps):
    model.Equations([x[:, k + 1] == x[:, k] + (dt/2.0)*(f(x, u, k + 1) + f(x, u, k))])

gives me the following error,

Exception: @error: Equation Definition
 Equation without an equality (=) or inequality (>,<)
 false
 STOPPING...

I've added two commented lines of code in the above code snippet that will allow the program to run correctly, but I'm hoping to avoid having to separate each equation out, since I'd like to extend this to problems that deal with more complicated system dynamics, and to also use more sophisticated collocation methods instead of the trapezoidal approach.

I know that GEKKO has some nice features for dynamic optimization, but I'm looking to try and implement various direct collocation methods myself to understand the theory a bit better.

Answer 1

There are some examples of orthogonal collocation on finite elements in the Machine Learning and Dynamic Optimization course.

from __future__ import division
import numpy as np
from scipy.optimize import fsolve
from scipy.integrate import odeint
import matplotlib.pyplot as plt

# final time
tf = 1.0

# solve with ODEINT (for comparison)
def model(x,t):
    u = 4.0
    return (-x**2 + u)/5.0
t = np.linspace(0,tf,20)
y0 = 0
y = odeint(model,y0,t)
plt.figure(1)
plt.plot(t,y,'r-',label='ODEINT')

# ----------------------------------------------------
# Approach #1 - Write the model equations in Python
# ----------------------------------------------------
# define collocation matrices
def colloc(n):
    if (n==2):
        NC = np.array([[1.0]])
    if (n==3):
        NC = np.array([[0.75,-0.25], \
                       [1.00, 0.00]])
    if (n==4):
        NC = np.array([[0.436,-0.281, 0.121], \
                       [0.614, 0.064, 0.0461], \
                       [0.603, 0.230, 0.167]])
    if (n==5):
        NC = np.array([[0.278, -0.202, 0.169, -0.071], \
                       [0.398,  0.069, 0.064, -0.031], \
                       [0.387,  0.234, 0.278, -0.071], \
                       [0.389,  0.222, 0.389,  0.000]])
    if (n==6):
        NC = np.array([[0.191, -0.147, 0.139, -0.113, 0.047],
                       [0.276,  0.059, 0.051, -0.050, 0.022],
                       [0.267,  0.193, 0.252, -0.114, 0.045],
                       [0.269,  0.178, 0.384,  0.032, 0.019],
                       [0.269,  0.181, 0.374,  0.110, 0.067]])
    return NC

# define collocation points from Lobatto quadrature
def tc(n):
    if (n==2):
        time = np.array([0.0,1.0])
    if (n==3):
        time = np.array([0.0,0.5,1.0])
    if (n==4):
        time = np.array([0.0, \
                         0.5-np.sqrt(5)/10.0, \
                         0.5+np.sqrt(5)/10.0, \
                         1.0])
    if (n==5):
        time = np.array([0.0,0.5-np.sqrt(21)/14.0, \
                         0.5,0.5+np.sqrt(21)/14.0, 1])
    if (n==6):
        time = np.array([0.0, \
                         0.5-np.sqrt((7.0+2.0*np.sqrt(7.0))/21.0)/2.0, \
                         0.5-np.sqrt((7.0-2.0*np.sqrt(7.0))/21.0)/2.0, \
                         0.5+np.sqrt((7.0-2.0*np.sqrt(7.0))/21.0)/2.0, \
                         0.5+np.sqrt((7.0+2.0*np.sqrt(7.0))/21.0)/2.0, \
                         1.0])
    return time*tf

# solve with SciPy fsolve
def myFunction(z,*param):
    n = param[0]
    m = param[1]
    # rename z as x and xdot variables
    x = np.empty(n-1)
    xdot = np.empty(n-1)
    x[0:n-1] = z[0:n-1]
    xdot[0:n-1] = z[n-1:m]

    # initial condition (x0)
    x0 = 0.0
    # input parameter (u)
    u = 4.0
    # final time
    tn = tf

    # function evaluation residuals
    F = np.empty(m)
    # nonlinear differential equations at each node
    # 5 dx/dt = -x^2 + u
    F[0:n-1] = 5.0 * xdot[0:n-1] + x[0:n-1]**2 - u
    # collocation equations
    # tn * NC * xdot = x - x0
    NC = colloc(n)
    F[n-1:m] = tn * np.dot(NC,xdot) - x + x0 * np.ones(n-1)
    return F

sol_py = np.empty(5) # store 5 results
for i in range(2,7):
    n = i
    m = (i-1)*2
    zGuess = np.ones(m)
    z = fsolve(myFunction,zGuess,args=(n,m))
    # add to plot
    yc = np.insert(z[0:n-1],0,0)
    plt.plot(tc(n),yc,'o',markersize=10,label='Nodes = '+str(i))
    # store just the last x[n] value
    sol_py[i-2] = z[n-2]
plt.legend(loc='best')

# ----------------------------------------------------
# Approach #2 - Write model in APMonitor and let
#   modeling language create the collocation equations
# ----------------------------------------------------
# load GEKKO
from gekko import GEKKO

sol_apm = np.empty(5) # store 5 results
i = 0
for nodes in range(2,7):
    m = GEKKO(remote=False)

    u = m.Param(value=4)
    x = m.Var(value=0)
    m.Equation(5*x.dt() == -x**2 + u)

    m.time = [0,tf]

    m.options.imode = 4
    m.options.time_shift = 0
    m.options.nodes = nodes

    m.solve() # solve problem
    sol_apm[i] = x.value[-1] # store solution (last point)
    i += 1

# print the solutions
print(sol_py)
print(sol_apm)

# show plot
plt.ylabel('x(t)')
plt.xlabel('time')
plt.show()

You can define variables with the same name (such as x) or use m.Array(m.Var,n) to define the variables. One thing to look at is the model file by opening the run folder with m.open_folder() before you send the m.solve() command. Look at the .apm file in that folder with a text editor.