T_max_Single = 7607000/9

g = 9.81 
m = 114239.00
k = 18.74
I = 16071705.59

import numpy as np
import math
import control as ct
import control.optimal as opt
import matplotlib.pyplot as plt
import logging
import time
import os
plt.style.use("default")
from IPython.display import display
from colorama import *
import sympy as s
from scipy.integrate import odeint


#Definition of the states
def rocket_update(t, x, u, params):
        
    xpos = x[0] 
    U = x[1] 
    z = x[2] 
    W = x[3] 
    θ = x[4] 
    q = x[5] 
    
    T = u[0]
    a = u[1]

    # Return the derivative of the state
    dydt = np.array([U,
            T*np.cos(θ-a)/m, 
            W,
            T*np.sin(θ-a)/m-g,
            q,
            T*k*np.sin(a)/I])
    
    return(dydt)

def rocket_output(t, x, u, params):
    return x                            # return (full state)



Tf = 8



# Define the rocket movement dynamics as an input/output system
rocket = ct.NonlinearIOSystem(
    rocket_update, rocket_output, states=6, name='rocket',
    inputs=("T", "α"), outputs=("x", "U", "z", "W", "θ", "q")) 

# Define the time horizon (and spacing) for the optimization
horizon = np.linspace(0, Tf, Tf, endpoint=True)                                        #############IMPORTANT

# Provide an intial guess (will be extended to entire horizon)
bend_left = [T_max_Single, 0.01]          # slight left veer                     #############IMPORTANT


# Optimal Control Problem
Initial_x = 0 #m
Final_x =  5 #m

Initial_z =  0 #m
Final_z = 200 #m

Initial_θ = np.pi/2 #m
Final_θ = np.pi/3 #m

Final_Thrust = T_max_Single

x0 = [Initial_x, 0, Initial_z, 0, Initial_θ, 0]; u0 = [bend_left[0], bend_left[1]]
xf = [Final_x, 0, Final_z, 0, Final_θ, 0]; uf = [Final_Thrust, 0]

#T_max_Single
Min_T = 1 * T_max_Single
Max_T = 9 * T_max_Single
a_min = -20* np.pi/180
a_max = 20* np.pi/180

# Approach 3: terminal constraints
#
# We can also remove the cost function on the state and replace it
# with a terminal *constraint* on the state.  If a solution is found,
# it guarantees we get to exactly the final state.
#
print("Initialisation of optimisation")    
# print("Checkpoint_1")

# Input cost and terminal constraints
R = np.diag([0, 0])                 # minimize applied inputs
cost3 = opt.quadratic_cost(rocket, np.zeros((6,6)), R, u0=uf)

con1 = [ opt.input_range_constraint(rocket, [Min_T, a_min], [Max_T, a_max]) ]

terminal = [ opt.state_range_constraint(rocket, [Final_x, -np.inf, Final_z, -np.inf, -np.inf, -np.inf],
                                                [Final_x, np.inf, Final_z, np.inf, np.inf, np.inf]) ]


# Reset logging to its default values
logging.basicConfig(
    level=logging.DEBUG, filename="./steering-terminal_constraint.log",
    filemode='w', force=True)

# Compute the optimal control
start_time = time.process_time()
result3 = opt.solve_ocp(
    rocket, horizon, x0, cost3, con1,          
    terminal_constraints=terminal, initial_guess=bend_left, log=False, method="trust-constr",
    options={'eps': 0.01})

print("* Total time = %5g seconds\n" % (time.process_time() - start_time))

/opt/anaconda3/lib/python3.8/site-packages/scipy/optimize/_constraints.py:386: OptimizeWarning: At least one constraint is unbounded above and below. Such constraints are ignored.
  warn("At least one constraint is unbounded above and below. Such "
/opt/anaconda3/lib/python3.8/site-packages/scipy/optimize/_constraints.py:432: OptimizeWarning: Equality and inequality constraints are specified in the same element of the constraint list. For efficient use with this method, equality and inequality constraints should be specified in separate elements of the constraint list. 
  warn("Equality and inequality constraints are specified in the same "