2

I am trying to optimize the trajectory of a thrust propelled system. The control variable is the mass flow rate, and the final objective is to maximize the mass of the robot, minimizing the amount of propelled used. The trajectory resembles a ballistic one, with an initial ascent phase and a final descent phase.

I think i managed to get a good initial guess, however the algorithm does not converge. I checked the output in the console and it seems that the objective function is not working correctly, and I think this is why it is not converging.

Here is my code

~# -*- coding: utf-8 -*-

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

# create GEKKO model
m = GEKKO(remote=False)
# m = GEKKO()
print(m.path)               
g_0 = 9.81 

Isp = 46.28 

m_fb=1 
m_prop=0.1*m_fb 
m_dry=value=m_fb-m_prop

c = 0.8 
a = 0.4

g = 1.62 
R_moon = 1737.4e3 

J_y = 0.06666 


m_dot_up=2.5*m_fb*g/(Isp*g_0) 
m_dot_low=0*m_fb*g/(Isp*g_0) 


theta_0=70*np.pi/180 
v0= 3   
x_f=np.sin(2*theta_0)*v0*v0/g
v0_x= v0*np.cos(theta_0) 
y_max=v0*v0/(2*g)
m0=1
m1=(np.e**(-v0/(Isp*g_0)))    
time_burn= m_fb*(m0-m1)/m_dot_up
tf=2*v0*np.sin(theta_0)/g + 2*time_burn

t0=0
nr_intervals=30
step=tf/nr_intervals
t=np.linspace(t0, tf, nr_intervals) 
time_burn_node= min(range(len(t)), key=lambda i: abs(t[i]-time_burn))
m.time=t

m0=1
m1=(np.e**(-v0/(Isp*g_0)))    
time_burn= m_fb*(m0-m1)/m_dot_up 
time_burn_node= min(range(len(t)), key=lambda i: abs(t[i]-time_burn)) 

y_f=1
v_f_x = 1 
v_f_y=3 
gamma_f = -90*np.pi/180 
alpha_f= 180*np.pi/180
alpha_dot_f=5*np.pi/180 
parabola_profile= lambda x: -4*y_max/(x_f*x_f)*x*x+4*y_max*x/x_f  
velocity_profile_y=lambda t: (v0*np.sin(theta_0)-g*t) 
velocity_profile_x= lambda t: v0*np.cos(theta_0) 
velocity_profile=lambda t: np.sqrt(velocity_profile_x(t)*velocity_profile_x(t)+velocity_profile_y(t)*velocity_profile_y(t))

x2 = m.Var(value=[np.linspace(0,v0,time_burn_node+1)[int(i)] for i in np.linspace(0,time_burn_node,time_burn_node+1)]+[velocity_profile(i) for i in t[1:int(nr_intervals)-time_burn_node]],lb=0,ub=1e3) 

x3 = m.Var(value=[np.linspace(np.pi/2,theta_0,time_burn_node+1)[int(i)] for i in np.linspace(0,time_burn_node,time_burn_node+1)]+[np.arctan2(velocity_profile_y(i),velocity_profile_x(i)) for i in t[1:int(nr_intervals)-2*time_burn_node-1]]+[np.linspace(np.arctan2(velocity_profile_y(t[int(nr_intervals)-2*time_burn_node]),velocity_profile_x(t[int(nr_intervals)-2*time_burn_node])),-np.pi/2,time_burn_node+1)[int(i)] for i in np.linspace(0,time_burn_node,time_burn_node+1)],lb=-np.pi*2,ub=np.pi*2) 

x0 = m.Var(value=[np.trapz(np.multiply(x2.value[0:i],np.cos(x3.value[0:i])),x=t[0:i]) for i in range(0,int(nr_intervals))],lb=0,ub=x_f+1) 
x1 = m.Var(value=[np.trapz(np.multiply(x2.value[0:i],np.sin(x3.value[0:i])),x=t[0:i]) for i in range(0,int(nr_intervals))],lb=0,ub=x_f+15) 
x4 = m.Var(value=np.concatenate((np.zeros(int(nr_intervals/2)),np.pi*np.ones(int(nr_intervals)-int(nr_intervals/2)))),lb=-np.pi*2,ub=np.pi*2) 
cc= [x*180/3.1415 for x in x4.value]

x5 = m.Var(value=[(x4.value[i+1]-x4.value[i])/step for i in range(0,len(x4.value)-1)]+[(x4[-1]-x4[-2])/step],lb=-step/J_y,ub=step/J_y) 
x6 = m.Var(value=np.concatenate((np.linspace(m0,m1,time_burn_node),m1*np.ones(len(t)-time_burn_node))),lb=0,ub=m0) 

x2 = m.Var(value=[np.linspace(0,v0,time_burn_node+1)[int(i)] for i in np.linspace(0,time_burn_node,time_burn_node+1)]+[velocity_profile(i) for i in t[1:int(nr_intervals)-time_burn_node]],lb=0,ub=1e3)
x3 = m.Var(value=[np.linspace(np.pi/2,theta_0,time_burn_node+1)[int(i)] for i in np.linspace(0,time_burn_node,time_burn_node+1)]+[np.arctan2(velocity_profile_y(i),velocity_profile_x(i)) for i in t[1:int(nr_intervals)-2*time_burn_node-1]]+[np.linspace(np.arctan2(velocity_profile_y(t[int(nr_intervals)-2*time_burn_node]),velocity_profile_x(t[int(nr_intervals)-2*time_burn_node])),-np.pi/2,time_burn_node+1)[int(i)] for i in np.linspace(0,time_burn_node,time_burn_node+1)],lb=-np.pi*2,ub=np.pi*2) 
x0 = m.Var(value=[np.trapz(np.multiply(x2.value[0:i],np.cos(x3.value[0:i])),x=t[0:i]) for i in range(0,int(nr_intervals))],lb=0,ub=x_f+1) 
x1 = m.Var(value=[np.trapz(np.multiply(x2.value[0:i],np.sin(x3.value[0:i])),x=t[0:i]) for i in range(0,int(nr_intervals))],lb=0,ub=x_f+15) 
x4 = m.Var(value=np.concatenate((np.zeros(int(nr_intervals/2)),np.pi*np.ones(int(nr_intervals)-int(nr_intervals/2)))),lb=-np.pi*2,ub=np.pi*2) 
x5 = m.Var(value=[(x4.value[i+1]-x4.value[i])/step for i in range(0,len(x4.value)-1)]+[(x4[-1]-x4[-2])/step],lb=-step/J_y,ub=step/J_y) 
x6 = m.Var(value=np.concatenate((np.linspace(m0,m1,time_burn_node),m1*np.ones(len(t)-time_burn_node))),lb=0,ub=m0) 

m_dot= m.MV(value=np.concatenate((m_dot_up*np.ones(time_burn_node),np.zeros(len(t)-time_burn_node))),lb=m_dot_low,ub=m_dot_up)
m_dot.value=m_dot.value[0:len(x1.value)]

p = np.zeros(len(x1.value)) 
p[-1]=1
final = m.Param(value=p,lb=0,ub=1)
m.Equation(x0.dt()==x2*m.cos(x3))
m.Equation(x1.dt()==x2*m.sin(x3))
m.Equation(x6*x2.dt()==(Isp*m_dot*g_0)*m.cos(x4)-x6*g*m.sin(x3))
m.Equation(x6*x2*x3.dt()==x2**2*m.cos(x3)*x6/R_moon+(Isp*m_dot*g_0)*m.sin(x4)-x6*g*m.cos(x3))
m.Equation(x6.dt()==-m_dot)


m.fix_final(x0,x0.value[-1])
m.Equation(x1*final<=1)
m.Minimize((-x6*final))

m.options.MAX_ITER = 1000     # adjust maximum iterations
m.options.SOLVER = 3
m.options.IMODE = 6
m.options.NODES = 3

m.solve()
print(final.value)
print(f"Final Mass:     {x6.value[-1]:.3f} s")

Here is the output of the console, showing the objective function going from negative to positive, which makes no sense as the final mass is always positive

iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  90r-8.8685190e-001 2.32e+000 9.25e+003   0.9 3.41e+001    -  1.18e-001 2.17e-002f  1
  91 -8.8659478e-001 3.03e+000 1.79e+005   0.3 5.68e+001    -  2.78e-001 1.27e-001f  2
  92 -8.8283316e-001 6.47e+000 2.86e+007   1.6 1.16e+002    -  9.91e-001 4.76e-002f  1
  93 1.8507003e+000 1.87e+001 1.55e+007   1.6 5.71e+003    -  2.41e-001 3.44e-001f  1
  94 1.8506996e+000 1.86e+001 1.16e+011   1.6 1.25e+001  10.6 6.17e-003 2.28e-003h  1
  95 1.8506996e+000 1.86e+001 1.16e+011   1.6 1.25e+001  10.1 5.75e-002 2.28e-005h  1

I have also outputted the final values for the final vector and final mass, and they are showing the correct results:

[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
Final Mass:     0.993 s

Thank you very much for any suggestion

João Gambôa
  • 23
  • 2

1 Answers1

0

Examine the model file with m.open_folder() and view the gk0_model.apm file in a text editor.

Model
Parameters
    p1<= 0.008920571233734825, >= 0.0
    p2<= 1, >= 0
End Parameters
Variables
    v1<= 1000.0, >= 0
    v2<= 6.283185307179586, >= -6.283185307179586
    v3<= 4.57104227603633, >= 0
    v4<= 18.57104227603633, >= 0
    v5<= 6.283185307179586, >= -6.283185307179586
    v6<= 2.478718169538892, >= -2.478718169538892
    v7<= 1, >= 0
    v8<= 1000.0, >= 0
    v9<= 6.283185307179586, >= -6.283185307179586
    v10<= 4.57104227603633, >= 0
    v11<= 18.57104227603633, >= 0
    v12<= 6.283185307179586, >= -6.283185307179586
    v13<= 2.478718169538892, >= -2.478718169538892
    v14<= 1, >= 0
End Variables
Equations
    $v10=((v8)*(cos(v9)))
    $v11=((v8)*(sin(v9)))
    ((v14)*($v8))=(((((((46.28)*(p1)))*(9.81)))*(cos(v12)))-((((v14)*(1.62)))*(sin(v9))))
    ((((v14)*(v8)))*($v9))=((((((((((v8)^(2)))*(cos(v9))))*(v14)))/(1737400.0))+((((((46.28)*(p1)))*(9.81)))*(sin(v12))))-((((v14)*(1.62)))*(cos(v9))))
    $v14=(-p1)
    ((v11)*(p2))<=1
    minimize (((-v14))*(p2))
End Equations
Connections
    p(end).n(end).v10=4.25390890270255
    p(end).n(end).v10=fixed
End Connections

End Model

The objective is minimize (((-v14))*(p2)) and the solver is not constrained to take a feasible search path to the optimum. The value of v14 is likely oscillating between positive and negative values.

Initialization and softening the constraints can help find a solution. Try replacing the hard terminal constraints with soft constraints such as:

#m.fix_final(x0,x0.value[-1])
#m.Equation(x1*final<=1)
m.Minimize(final*(x0-x0.value[-1])**2)
m.Minimize(final*(x1-1)**2)

Another strategy is to first solve for a feasible solution where the decision variables are fixed at reasonable values. Setting m.options.COLDSTART=1 turns off the MVs to find a feasible solution.

# -*- coding: utf-8 -*-

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

# create GEKKO model
m = GEKKO(remote=False)
# m = GEKKO()
print(m.path)               
g_0 = 9.81 

Isp = 46.28 

m_fb=1 
m_prop=0.1*m_fb 
m_dry=value=m_fb-m_prop

c = 0.8 
a = 0.4

g = 1.62 
R_moon = 1737.4e3 

J_y = 0.06666 


m_dot_up=2.5*m_fb*g/(Isp*g_0) 
m_dot_low=0*m_fb*g/(Isp*g_0) 


theta_0=70*np.pi/180 
v0= 3   
x_f=np.sin(2*theta_0)*v0*v0/g
v0_x= v0*np.cos(theta_0) 
y_max=v0*v0/(2*g)
m0=1
m1=(np.e**(-v0/(Isp*g_0)))    
time_burn= m_fb*(m0-m1)/m_dot_up
tf=2*v0*np.sin(theta_0)/g + 2*time_burn

t0=0
nr_intervals=30
step=tf/nr_intervals
t=np.linspace(t0, tf, nr_intervals) 
time_burn_node= min(range(len(t)), key=lambda i: abs(t[i]-time_burn))
m.time=t

m0=1
m1=(np.e**(-v0/(Isp*g_0)))    
time_burn= m_fb*(m0-m1)/m_dot_up 
time_burn_node= min(range(len(t)), key=lambda i: abs(t[i]-time_burn)) 

y_f=1
v_f_x = 1 
v_f_y=3 
gamma_f = -90*np.pi/180 
alpha_f= 180*np.pi/180
alpha_dot_f=5*np.pi/180 
parabola_profile= lambda x: -4*y_max/(x_f*x_f)*x*x+4*y_max*x/x_f  
velocity_profile_y=lambda t: (v0*np.sin(theta_0)-g*t) 
velocity_profile_x= lambda t: v0*np.cos(theta_0) 
velocity_profile=lambda t: np.sqrt(velocity_profile_x(t)*velocity_profile_x(t)+velocity_profile_y(t)*velocity_profile_y(t))

x2 = m.Var(value=[np.linspace(0,v0,time_burn_node+1)[int(i)] for i in np.linspace(0,time_burn_node,time_burn_node+1)]+[velocity_profile(i) for i in t[1:int(nr_intervals)-time_burn_node]],lb=0,ub=1e3) 

x3 = m.Var(value=[np.linspace(np.pi/2,theta_0,time_burn_node+1)[int(i)] for i in np.linspace(0,time_burn_node,time_burn_node+1)]+[np.arctan2(velocity_profile_y(i),velocity_profile_x(i)) for i in t[1:int(nr_intervals)-2*time_burn_node-1]]+[np.linspace(np.arctan2(velocity_profile_y(t[int(nr_intervals)-2*time_burn_node]),velocity_profile_x(t[int(nr_intervals)-2*time_burn_node])),-np.pi/2,time_burn_node+1)[int(i)] for i in np.linspace(0,time_burn_node,time_burn_node+1)],lb=-np.pi*2,ub=np.pi*2) 

x0 = m.Var(value=[np.trapz(np.multiply(x2.value[0:i],np.cos(x3.value[0:i])),x=t[0:i]) for i in range(0,int(nr_intervals))],lb=0,ub=x_f+1) 
x1 = m.Var(value=[np.trapz(np.multiply(x2.value[0:i],np.sin(x3.value[0:i])),x=t[0:i]) for i in range(0,int(nr_intervals))],lb=0,ub=x_f+15) 
x4 = m.Var(value=np.concatenate((np.zeros(int(nr_intervals/2)),np.pi*np.ones(int(nr_intervals)-int(nr_intervals/2)))),lb=-np.pi*2,ub=np.pi*2) 
cc= [x*180/3.1415 for x in x4.value]

x5 = m.Var(value=[(x4.value[i+1]-x4.value[i])/step for i in range(0,len(x4.value)-1)]+[(x4[-1]-x4[-2])/step],lb=-step/J_y,ub=step/J_y) 
x6 = m.Var(value=np.concatenate((np.linspace(m0,m1,time_burn_node),m1*np.ones(len(t)-time_burn_node))),lb=0,ub=m0) 

x2 = m.Var(value=[np.linspace(0,v0,time_burn_node+1)[int(i)] for i in np.linspace(0,time_burn_node,time_burn_node+1)]+[velocity_profile(i) for i in t[1:int(nr_intervals)-time_burn_node]],lb=0,ub=1e3)
x3 = m.Var(value=[np.linspace(np.pi/2,theta_0,time_burn_node+1)[int(i)] for i in np.linspace(0,time_burn_node,time_burn_node+1)]+[np.arctan2(velocity_profile_y(i),velocity_profile_x(i)) for i in t[1:int(nr_intervals)-2*time_burn_node-1]]+[np.linspace(np.arctan2(velocity_profile_y(t[int(nr_intervals)-2*time_burn_node]),velocity_profile_x(t[int(nr_intervals)-2*time_burn_node])),-np.pi/2,time_burn_node+1)[int(i)] for i in np.linspace(0,time_burn_node,time_burn_node+1)],lb=-np.pi*2,ub=np.pi*2) 
x0 = m.Var(value=[np.trapz(np.multiply(x2.value[0:i],np.cos(x3.value[0:i])),x=t[0:i]) for i in range(0,int(nr_intervals))],lb=0,ub=x_f+1) 
x1 = m.Var(value=[np.trapz(np.multiply(x2.value[0:i],np.sin(x3.value[0:i])),x=t[0:i]) for i in range(0,int(nr_intervals))],lb=0,ub=x_f+15) 
x4 = m.Var(value=np.concatenate((np.zeros(int(nr_intervals/2)),np.pi*np.ones(int(nr_intervals)-int(nr_intervals/2)))),lb=-np.pi*2,ub=np.pi*2) 
x5 = m.Var(value=[(x4.value[i+1]-x4.value[i])/step for i in range(0,len(x4.value)-1)]+[(x4[-1]-x4[-2])/step],lb=-step/J_y,ub=step/J_y) 
x6 = m.Var(value=np.concatenate((np.linspace(m0,m1,time_burn_node),m1*np.ones(len(t)-time_burn_node))),lb=0,ub=m0) 

m_dot= m.MV(value=np.concatenate((m_dot_up*np.ones(time_burn_node),np.zeros(len(t)-time_burn_node))),lb=m_dot_low,ub=m_dot_up)
m_dot.value=m_dot.value[0:len(x1.value)]

p = np.zeros(len(x1.value)) 
p[-1]=1
final = m.Param(value=p,lb=0,ub=1)
m.Equation(x0.dt()==x2*m.cos(x3))
m.Equation(x1.dt()==x2*m.sin(x3))
m.Equation(x6*x2.dt()==(Isp*m_dot*g_0)*m.cos(x4)-x6*g*m.sin(x3))
m.Equation(x6*x2*x3.dt()==x2**2*m.cos(x3)*x6/R_moon+(Isp*m_dot*g_0)*m.sin(x4)-x6*g*m.cos(x3))
m.Equation(x6.dt()==-m_dot)

#m.fix_final(x0,x0.value[-1])
#m.Equation(x1*final<=1)
m.Minimize(final*(x0-x0.value[-1])**2)
m.Minimize(final*(x1-1)**2)
m.Minimize((-x6*final))

m.options.MAX_ITER = 2000     # adjust maximum iterations
m.options.SOLVER = 2
m.options.IMODE = 6
m.options.NODES = 3

m.open_folder()

m.options.COLDSTART = 1
m.solve()
print(final.value)
print(f"Final Mass:     {x6.value[-1]:.3f} s")

This gives an error:

 Number of state variables:           1247
 Number of total equations: -          725
 Number of slack variables: -            0
 ---------------------------------------
 Degrees of freedom       :            522
 
 @error: Degrees of Freedom
 * Error: DOF must be zero for this mode
 STOPPING...
Traceback (most recent call last):
  File "C:\Users\johnh\Desktop\test.py", line 111, in <module>
    m.solve()
  File "C:\Users\johnh\Python39\lib\site-packages\gekko\gekko.py", line 2185, in solve
    raise Exception(response)
Exception:  @error: Degrees of Freedom
 * Error: DOF must be zero for this mode
 STOPPING...

Try removing any constraints and identify the pairing of each variable with a constraint. If the degrees of freedom are zero (same number of equations and variables), then the solver can sometimes find an initial solution that can subsequently be optimized.

John Hedengren
  • 12,068
  • 1
  • 21
  • 25