import numpy as np
import matplotlib.pyplot as plt
from gekko import GEKKO
# This optimizer will minimize time to get to desired end points
# Next I will need to add to the cost function the error between the rocket and the desired set point
class Optimizer():
def __init__(self):
self.d = GEKKO(remote=True) # Driving on ground optimizer
ntd = 51 # Number of time discretizations
self.d.time = np.linspace(0, 1, ntd) # Time vector normalized 0-1
# options
self.d.options.NODES = 3
self.d.options.SOLVER = 3
self.d.options.IMODE = 6# MPC mode
self.d.options.MAX_ITER = 800
self.d.options.MV_TYPE = 0
self.d.options.DIAGLEVEL = 0
# final time for driving optimizer
self.tf = self.d.FV(value=1.0,lb=1,ub=100.0, name='tf')
# allow gekko to change the tf value
self.tf.STATUS = 1
# Acceleration variable manipulatable
self.a = self.d.MV(fixed_initial=False, lb = -1, ub = 1, name='a')
self.a.STATUS = 1
# Picker vector to pick out states for objective function
# This one picks the last state by setting the vector to all 0's except the last element
self.p_d = np.zeros(ntd)
self.p_d[-1] = 1.0
self.final = self.d.Param(value = self.p_d, name='final')
# Magnitude of acceleration
self.thr_mag = self.d.Param(value = 996, name='thr_mag')
def optimize1D(self, s, v, sf, vf): #these are 1x2 vectors s or v [x, z]
# variables intial conditions are placed here
self.s = self.d.Var(value=s, lb=-4096, ub=4096, name='pos') #x position
self.v = self.d.Var(value=v, lb=-5120, ub=5120, name='vel') #y position
# Differental equations
self.d.Equation(self.v.dt()/self.tf == (self.a * self.thr_mag))
self.d.Equation(self.s.dt()/self.tf == self.v)
# Cost functions (minimizing)
# Minimze final desired state with state variables
self.d.Obj(self.final*1e4*((self.s-sf)**2)) # Soft constraints
self.d.Obj(self.final*1e4*((self.v-vf)**2))
#Objective function to minimize time
self.d.Obj(1e1*self.tf)
#solve
# self.d.solve('http://127.0.0.1') # Solve with local apmonitor server
self.d.open_folder()
self.d.solve(disp=True)
self.ts = np.multiply(self.d.time, self.tf.value[0])
return self.ts
def getTrajectoryData(self):
return [self.ts, self.sx, self.sy, self.vx, self.vy, self.yaw, self.omega]
def getInputData(self):
return [self.ts, self.a]
# Main Code
opt = Optimizer()
s = 10
v = 100
sf = 500
vf = 0
opt.optimize1D(s, v, sf, vf)
ts = opt.d.time * opt.tf.value[0]
# plot results
plt.figure(1)
plt.subplot(3,1,1)
plt.plot(ts, opt.a, 'r-')
plt.ylabel('acceleration')
plt.subplot(3,1,2)
plt.plot(ts, opt.s, 'r-')
plt.ylabel('pos')
plt.subplot(3, 1, 3)
plt.plot(ts, opt.v, 'b-')
plt.ylabel('vel')
plt.xlabel('time')
plt.show()
apm 67.243.14.146_gk_model11 <br><pre> ---------------------------------------------------------------- APMonitor, Version 1.0.0 APMonitor Optimization Suite ---------------------------------------------------------------- --------- APM Model Size ------------ Each time step contains Objects : 0 Constants : 0 Variables : 6 Intermediates: 0 Connections : 1 Equations : 5 Residuals : 5 Number of state variables: 702 Number of total equations: - 650 Number of slack variables: - 0 --------------------------------------- Degrees of freedom : 52 ********************************************** Dynamic Control with Interior Point Solver ********************************************** Info: Exact Hessian ****************************************************************************** This program contains Ipopt, a library for large-scale nonlinear optimization. Ipopt is released as open source code under the Eclipse Public License (EPL). For more information visit http://projects.coin-or.org/Ipopt ****************************************************************************** This is Ipopt version 3.12.10, running with linear solver ma57. Number of nonzeros in equality constraint Jacobian...: 1646 Number of nonzeros in inequality constraint Jacobian.: 200 Number of nonzeros in Lagrangian Hessian.............: 401 Total number of variables............................: 702 variables with only lower bounds: 100 variables with lower and upper bounds: 302 variables with only upper bounds: 0 Total number of equality constraints.................: 550 Total number of inequality constraints...............: 100 inequality constraints with only lower bounds: 100 inequality constraints with lower and upper bounds: 0 inequality constraints with only upper bounds: 0 iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls 0 2.5010010e+09 1.00e+02 1.00e+00 0.0 0.00e+00 - 0.00e+00 0.00e+00 0 1 2.4680415e+09 1.01e+02 1.11e+01 -3.9 3.79e+02 -2.0 2.68e-04 3.83e-03f 1 2 2.0051325e+09 4.30e+02 5.80e+00 -0.0 1.12e+01 -0.7 1.57e-01 1.00e+00f 1 3 1.9987968e+09 2.69e+02 2.52e+01 -0.1 2.08e+01 -0.2 8.18e-01 1.95e-01f 1 4 1.8596700e+09 1.03e+02 1.85e+00 0.4 5.89e+00 -0.7 5.75e-01 1.00e+00f 1 5 1.4827347e+09 1.10e+02 2.82e+00 -0.1 1.00e+01 -1.2 9.53e-01 1.00e+00f 1 6 7.6581844e+08 2.98e+02 4.24e+00 -0.8 2.94e+01 -1.7 7.57e-01 1.00e+00f 1 7 6.0087428e+08 2.46e+01 1.03e+00 -6.4 1.30e+01 -1.3 5.93e-01 1.00e+00f 1 8 3.0121286e+08 3.34e+01 1.03e+00 -1.8 1.51e+01 -1.7 9.97e-01 1.00e+00f 1 9 2.2595097e+08 3.39e+00 2.27e-01 -2.5 4.50e+00 -1.3 9.93e-01 1.00e+00f 1 iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls 10 1.0617011e+08 4.89e+00 1.38e-01 -3.8 7.91e+00 -1.8 1.00e+00 1.00e+00f 1 11 1.8428181e+07 1.24e+01 6.65e-02 -4.3 1.23e+01 -2.3 1.00e+00 1.00e+00f 1 12 6.7289223e+05 9.64e+00 2.14e-02 -5.6 1.29e+01 -2.7 1.00e+00 1.00e+00f 1 13 4.0924155e+04 6.10e-01 3.47e-03 -6.8 5.27e+00 -3.2 1.00e+00 1.00e+00h 1 14 2.7573035e+04 6.12e-02 5.03e-04 -8.4 2.61e+00 -3.7 1.00e+00 1.00e+00h 1 15 2.6103292e+04 3.63e-01 9.78e-05 -10.0 1.43e+00 -4.2 1.00e+00 1.00e+00h 1 16 2.2889876e+04 3.52e+00 9.57e-05 -11.0 4.20e+00 -4.6 1.00e+00 1.00e+00h 1 17 1.5870898e+04 3.15e+01 6.57e-05 -11.0 9.88e+00 -5.1 1.00e+00 1.00e+00h 1 18 8.6532393e+03 1.15e+02 5.74e-05 -11.0 1.50e+01 -5.6 1.00e+00 1.00e+00h 1 19 6.0555449e+03 9.76e+01 1.62e-05 -11.0 1.75e+01 -6.1 1.00e+00 1.00e+00h 1 iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls 20 4.2884214e+03 1.50e+02 7.55e-06 -9.0 2.48e+01 -6.5 1.00e+00 6.47e-01h 1 21 4.2781024e+03 1.49e+02 3.03e-05 -7.0 3.67e+01 -7.0 1.00e+00 3.63e-03h 1 22 4.0218713e+03 7.60e+01 3.20e-04 -5.0 2.82e+01 -6.6 1.00e+00 5.11e-01h 1 23 3.3710570e+03 1.15e+02 5.25e-05 -4.4 4.22e+01 -7.1 9.27e-01 1.00e+00h 1 24 1.8453154e+03 5.29e+02 6.06e-04 -4.7 5.12e+01 -7.6 8.39e-01 1.00e+00h 1 25 1.8017059e+03 1.44e+01 3.60e-04 -5.1 3.08e+01 - 9.38e-01 1.00e+00h 1 26 1.6397217e+03 1.69e+01 1.20e-04 -6.4 1.45e+01 - 9.60e-01 1.00e+00h 1 27 1.3196100e+03 1.35e+02 3.56e-05 -7.3 4.60e+01 - 7.76e-01 1.00e+00h 1 28 1.3241564e+03 1.43e+00 2.46e-07 -7.8 5.88e+01 - 8.36e-01 1.00e+00h 1 29 1.3111641e+03 5.20e+00 8.39e-08 -8.4 7.40e+01 - 8.86e-01 1.00e+00h 1 iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls 30 1.3103182e+03 6.03e-02 1.80e-08 -8.7 1.99e+02 - 8.38e-01 1.00e+00h 1 31 1.3093015e+03 4.61e-03 3.30e-09 -9.9 3.05e+02 - 9.34e-01 1.00e+00h 1 32 1.3091687e+03 5.67e-05 6.13e-13 -11.0 1.36e+02 - 1.00e+00 1.00e+00h 1 33 1.3091679e+03 1.28e-11 7.81e-16 -11.0 2.84e+00 - 1.00e+00 1.00e+00h 1 Number of Iterations....: 33 (scaled) (unscaled) Objective...............: 6.5458393585250707e-04 1.3091678717050142e+03 Dual infeasibility......: 7.8095671934531701e-16 1.5619134386906339e-09 Constraint violation....: 1.2898205484480532e-12 1.2846612662542611e-11 Complementarity.........: 1.3482889897266678e-11 2.6965779794533358e-05 Overall NLP error.......: 1.3482889897266678e-11 2.6965779794533358e-05 Number of objective function evaluations = 34 Number of objective gradient evaluations = 34 Number of equality constraint evaluations = 34 Number of inequality constraint evaluations = 34 Number of equality constraint Jacobian evaluations = 34 Number of inequality constraint Jacobian evaluations = 34 Number of Lagrangian Hessian evaluations = 33 Total CPU secs in IPOPT (w/o function evaluations) = 0.111 Total CPU secs in NLP function evaluations = 0.160 EXIT: Optimal Solution Found. The solution was found. The final value of the objective function is 1309.16787170501 --------------------------------------------------- Solver : IPOPT (v3.12) Solution time : 0.287199999991572 sec Objective : 1309.16787170501 Successful solution ---------------------------------------------------
File "<ipython-input-23-158f2f8c772a>", line 1 jupyter nbconvert "1D_Gekko_Solver.ipynb" --to pdf ^ SyntaxError: invalid syntax