Gravity Turn Maneuver (Casadi)

This page contains a discretized version of the OCP Gravity Turn Maneuver in CasADi 2.4.2 format. The continuous model was discretized using a direct multiple shooting approach with 300 shooting nodes distributed equidistantly over a variable time horizon. The control grid is equal to the DMS grid.

CasADi

# ----------------------------------------------------------------
# Gravity Turn Maneuver with direct multiple shooting using CVodes
# (c) Mirko Hahn
# ----------------------------------------------------------------
from casadi import *
 
N = 300
vel_eps = 1e-6
 
m0   = 11.3
m1   = 1.3
g0   = 9.81e-3
r0   = 6.0e2
Isp  = 300.0
Fmax = 600.0e-3
 
cd  = 0.021
A   = 1.0
H   = 5.6
rho = (1.0 * 1.2230948554874)
 
h_obj = 75
v_obj = 2.287
q_obj = 0.5 * pi
 
x = SX.sym('[m, v, q, h, d]')
u = SX.sym('u')
T = SX.sym('T')
 
ode = [
    -(Fmax / (Isp * g0)) * u,
    (Fmax * u - 0.5e3 * A * cd * rho * exp(-x[3] / H) * x[1]**2) / x[0] - g0 * (r0 / (r0 + x[3]))**2 * cos(x[2]),
    g0 * (r0 / (r0 + x[3]))**2 * sin(x[2]) / x[1] - x[1] * cos(x[2]) / (r0 + x[3]),
    x[1] * cos(x[2]),
    x[1] * sin(x[2]) / (r0 + x[3])
]
quad = u
 
dae = SXFunction("dae", daeIn(x=x, p=vertcat([u, T])), daeOut(ode=T*vertcat(ode), quad=T*quad))
I = Integrator("I", "cvodes", dae, {'t0': 0.0, 'tf': 1.0 / N})
 
p_min  = [120.0]
p_max  = [600.0]
p_init = [120.0]
 
u_min  = [0.0]
u_max  = [1.0]
u_init = [0.5]
 
x0_min  = [m0, vel_eps,       0.0, 0.0, 0.0]
x0_max  = [m0, vel_eps,  0.5 * pi, 0.0, 0.0]
x0_init = [m0, vel_eps, 0.05 * pi, 0.0, 0.0]
 
xf_min  = [m1, v_obj, q_obj, h_obj, 0.0]
xf_max  = [m0, v_obj, q_obj, h_obj, inf]
xf_init = [m1, v_obj, q_obj, h_obj, 0.0]
 
x_min  = [m1, vel_eps, 0.0, 0.0, 0.0]
x_max  = [m0,     inf,  pi, inf, inf]
x_init = [0.5 * (m0 + m1), 0.5 * v_obj, 0.5 * q_obj, 0.5 * h_obj, 0.0]
 
np = 1
nx = x.size1()
nu = u.size1()
ns = nx + nu
 
V = MX.sym('X', N * ns + nx + np)
P = V[0]
X = [V[np+i*ns:np+i*ns+nx] for i in range(0, N+1)]
U = [V[np+i*ns+nx:np+(i+1)*ns] for i in range(0, N)]
 
G = []
F = 0.0
 
x0 = p_init + x0_init
for i in range(0, N):
    Y = I({'x0': X[i], 'p': vertcat([U[i], P])})
    G = G + [Y['xf'] - X[i+1]]
    F = F + Y['qf']
 
    frac = float(i+1) / N
    x0 = x0 + u_init + [x0_init[i] + frac * (xf_init[i] - x0_init[i]) for i in range(0, nx)]
 
lbg = 0.0
ubg = 0.0
lbx = p_min + x0_min + u_min + (N-1) * (x_min + u_min) + xf_min
ubx = p_max + x0_max + u_max + (N-1) * (x_max + u_max) + xf_max
 
nlp = MXFunction("nlp", nlpIn(x=V), nlpOut(f=m0 - X[-1][0], g=vertcat(G)))
S = NlpSolver("S", "ipopt", nlp, {'tol': 1e-5})
r = S({
    'x0' : x0,
    'lbx': lbx,
    'ubx': ubx,
    'lbg': lbg,
    'ubg': ubg
})