DIRECT TRAJECTORY OPTIMIZATION OF UPRIGHT REUSABLE ROCKET LANDING

Christopher Iliffe Sprague


ABSTRACT

Recently within the aerospace industry, several demonstrations of successful soft, upright rocket landings have taken place; namely by SpaceX and Blue Origin. In this paper, such an application is examined through the scope of trajectory optimization and nonlinear programming. A general dynamical model of a rocket, equipped with a single thruster capable of gimbaling, is formulated. A direct method of discretizing state-time dynamics is applied to the dynamical model, from which a nonlinear program is developed. Using gradient based optimization, the nonlinear program representing the landing of a rocket is optimized with the objective of minimizing the rocket’s propellant expenditure, while abiding to the constraints of landing softly at the location of its target. It is shown that this paper’s dynamical model and trajectory optimization method yield optimal propellant expenditures comparable with that of literature.

Index Terms— Trajectory Optimization, Direct Methods

Contents

1 Introduction
2 Trajectory Optimization
 2.1 Indirect Methods
 2.2 Direct Methods
 2.3 Problem Description
 2.4 System Description
  2.4.1 State Variables
  2.4.2 Boundary Conditions
  2.4.3 Path Constraints
  2.4.4 System Bounds
  2.4.5 Path Cost
 2.5 Direct Transcription
  2.5.1 Grid
  2.5.2 Defects
  2.5.3 Nonlinear Programming Variables
 2.6 Trapezoidal Transcription
3 Rocket
 3.1 Control
 3.2 Rigid Body
  3.2.1 Orientation
  3.2.2 Inertia
 3.3 Dynamics
  3.3.1 Control
  3.3.2 Acceleration
 3.4 State
4 Results
5 Conclusions
6 Source Code

List of Tables

List of Figures

1 Direction Transcription Schematic
2 Convergence with 5 Nodes
3 Convergence with 10 Nodes
4 Trajectory with 5 Nodes
5 Trajectory with 10 Nodes
6 Trajectory with 15 Nodes
7 Trajectory with 25 Nodes

Listings

1 Introduction
2 Trajectory Optimization
 2.1 Indirect Methods
 2.2 Direct Methods
 2.3 Problem Description
 2.4 System Description
  2.4.1 State Variables
  2.4.2 Boundary Conditions
  2.4.3 Path Constraints
  2.4.4 System Bounds
  2.4.5 Path Cost
 2.5 Direct Transcription
  2.5.1 Grid
  2.5.2 Defects
  2.5.3 Nonlinear Programming Variables
 2.6 Trapezoidal Transcription
3 Rocket
 3.1 Control
 3.2 Rigid Body
  3.2.1 Orientation
  3.2.2 Inertia
 3.3 Dynamics
  3.3.1 Control
  3.3.2 Acceleration
 3.4 State
4 Results
5 Conclusions
6 Source Code

1.

1 Introduction

The applications of trajectory optimization range from lunar landings to robotic locomotion. Through the developments of space exploration over the past few decades, new methods of spacecraft trajectory optimization of come to surface. Trajectory optimization problems have been tackled from a variety of perspectives, for example: optimal control [?], machine learning [?], and convex programming [?]. This paper takes the perspective of gradient based optimization.

2.

2 Trajectory Optimization

Conventionally, in the case of a planetary lander, or in this case a rocket returning to Earth, trajectory optimization is performed using either direct or indirect methods. 2.1.

2.1 Indirect Methods

Indirect methods operate by first analytically constructing the necessary and sufficient conditions for optimality pertaining to the particular problem at hand, then secondly discretizing these conditions to formulate a constrained parameter optimization problem. Most often in the case of a planetary lander or the like, these necessary conditions are derived from Pontryagin’s minimum principle [?]. This principle states that, at every moment in time the dynamical system’s control should be chosen to maximize the Hamiltonian of the system. The reader is advised to consult [?] for my information about this method. 2.2.

2.2 Direct Methods

Direct methods operate by discretizing the trajectory optimization problem directly, in order to formulate a constrained parameter optimization problem. Direct transcription methods, in general, have several advantages over indirect transcription methods. The necessity to analytically determine initial costate variables is circumvented, and the optimization problem size is reduced by a factor of two. One of the most significant merits is that, one does not need to specify the structure of the problem, a priori. In general, direct transcription methods are very robust and are able to converge to optimal solutions from poor initial guesses.

2.3.

2.3 Problem Description

For a general dynamical system, the problem of trajectory optimization is formulated as

∫ tf m inui(mti)ze ϕ (ts,tf,xs,xf)+ t L(x(t),u(t),t)dt s subjectto x˙= a(x,t)+ u(t) dynam ic constraints G (ts,tf,xs,xf) ≤ 0boundary conditions u ∈ U(x,t)propulsion constraints

Essentially, this formal formulation asks for the control at every moment in time u(t) in order to minimize the initial and terminal costs ϕ(ts,tf,xs,xf) and the path costs ∫ tstfL(x(t),u(t),t)dt, all while abiding to dynamic constraints and boundary constraints, which are all dictated by the state of the dynamical system x(t). For the case of a rocket landing, it is quite obvious that the boundary constraints would mandate that the rocket’s final state x(tf) be equal to the rocket’s target state, which is chosen beforehand. The rocket’s dynamic constraints are not as intuitive, and are derived from the way in which the system is discretized.

2.4.

2.4 System Description

Following the common notation of optimal control, the trajectory of dynamical system can be formulated as a system of time-varying variables.

[x(t)] z = u(t)
consisting of the system’s state variables x(t) and the control variables u(t). Herein, vectors will be typeset in bold.

2.4.1.

2.4.1 State Variables

The way in which the dynamical system evolves over time is described by the system dynamics. These dynamics, for the scope of this paper’s discussion, are described by a system of ordinary differential equations, more commonly known as state equations. The state equations, in their generic form, are written as

dx- ˙x = dt = f(x(t),u(t),t)
Note that this description, in its generic form described a non-autonomous system, hence the right hand side’s explicit dependence on time t. In the case of many aerospace applications, the system of ordinary differential equations governing its dynamics does not explicitly depend on time. For example, in the case of a spacecraft on an interplanetary trajectory, its dynamics do not depend on time, but rather the inverse square of its distance relative to surrounding celestial bodies.

2.4.2.

2.4.2 Boundary Conditions

In the problem of trajectory optimization, two conditions must be strictly satisfied. That is, that the system’s initial dynamic variables

ψ0 = ψ(x(t0),u(t0),t0)
at the initial time t0, and the system’s terminal dynamics variables,
ψf = ψ(x(tf),u(tf),tf)
at the terminal time tf define the boundary conditions of the trajectory optimization problem.

2.4.3.

2.4.3 Path Constraints

In many dynamical systems, the problem of trajectory optimization leads itself to path constraints. These path constraints can range from obstacle avoidance in quad-copters to trajectory energy in spacecraft Lagrange point insertions. These path constraints can be formally stated as

g(x(t),u (t),t) = 0
Note, that this generic description of a trajectory optimization problem’s path constraints forms a strong analogue to equality constraints in design optimization.

2.4.4.

2.4.4 System Bounds

Of course, for many reasons it is necessary to bound a trajectory optimization problem, including, but not limited to: hardware constraints and simulation constraints. For example, one would bound the angular velocity of their fictional spacecraft so that it is not spinning into eternity. Additionally, one may want to place rectangular bounds on the environment of their spacecraft’s simulation, for example: a spacecraft’s position should be within the solar system. Aside from state constraints, there are also control constraints. In the case of the reusable rocket being examined in this paper, these constraints would be magnitude and direction of the rocket’s thrust. The bounding on the system’s state is formulated as

x ≤ x(t) ≤ x l u
, and on the system’s controls as
ul ≤ u(t) ≤ uu
.

2.4.5.

2.4.5 Path Cost

The whole point of trajectory optimization is select a control policy u(t) to minimize or maximize some measure of performance. In some cases, this measure of performance might be the time to travel from a starting location to an ending location in a dynamical system such as a quad copter. In the case of the rocket landing problem being investigated in this paper, the measure of performance is, of course, propellant expenditure. That is, the goal of the trajectory optimization process is to softly land the rocket at its target, while utilizing as little fuel as possible. For a general dynamical system, the path cost or objective function is defined as

J = ϕ(x(t0),x(tf),t0,tf)
In the case of a landing that reduces fuel usage, otherwise know as a mass optimal landing, the final mass will be used explicitly to determine the measure of performance.

2.5.

2.5 Direct Transcription

In direct transcription methods, the continuous state x(t) and control u(t) are discretized into a finite dimensional description. Through this technique, the original optimal control problem is transformed into a nonlinear programming problem (NLP). For the sake of intuition, transforming the nonlinear programming problem back to an optimal control problem would be akin to having an infinite quantity of states, controls, and constraints.

2.5.1.

2.5.1 Grid

The first step in transforming a continuous optimal control problem into a finite dimension nonlinear programming problem is to define the resolution of the discretization n, or the number of nodes. Including the statement of the dynamical system’s simulation starting time tI and final time tF. From these statements, one can formulate the time grid

[t1,t2.,...,tn]
, where one should note that tI = t1 and tF = tn, which form part of the system’s boundary conditions. Intuitively, it is followed that the states are discretized as
xi = x(ti)
and the controls as
ui = u(ti)

2.5.2.

2.5.2 Defects

Once the system’s original optimal control description has been transcribed through direct transcription, the values of the system’s states xi and controls ui are represented by a (possibly) large set of NLP variables. The aforementioned bounds on both the system’s state and control are simply instantiated at each node n in the discretization. Perhaps the most crucial part of this direct trajectory optimization process is enforcing the system’s inherent dynamics at each node. That is, an analytical quadrature procedure is used to compare the system’s states at collocated nodes, in order to asses the validity of the optimizer’s placement of each of the system’s states. The schematic of this process can be seen in Figure 1.

PIC

Fig. 1: Direction Transcription Schematic

[?]

2.5.3.
2.5.3 Nonlinear Programming Variables

As previously noted, under the direct transcription process, in which the system’s continuous state and control descriptions are transformed to a finite dimensional parameter constrained problem, a single NLP decision vector is dealt with. This decision vector is formulated as

⌊ ⌋ x0 || u0|| || x1|| || u1|| z = || ..|| || .|| || xn|| |⌈ un|⌉ t0 tf
, where t0 and tf are regarded as auxiliary variables. The auxiliary variables, in this case the time grid boundaries, determine the behaviour of the optimizer’s manipulation of the state and control variables, through the time step size passed to the analytical quadrature method.

2.6.

2.6 Trapezoidal Transcription

In order to compute the defects at each node in the grid space, a method to analytically propagate the system’s dynamics is used. In this paper, herein the method of trapezoidal quadrature will be used. Within the grid space, at node n, the defect is defined as

hn- ζn = xn - xn- 1 - 2 (fn + fn-1)
. Note that hn is the collocation step size, defined by
h = t - t n n n-1
, and fn is described by the system’s equations of motion at node n as
fn = f(x (tn),u(tn),tn)
. The resulting characterization of the defect forms the equality constraints of the dynamical system’s trajectory optimization problem. If these equality constraints are satisfied at each node, then the dynamics of the optimized trajectory will be feasible according to trapezoidal quadrature.

3.

3 Rocket

In order to optimize the trajectory of a rocket, it is first necessary to construct a dynamical model, which accurately describes a rocket’s dynamics. 3.1.

3.1 Control

It is appropriate to begin with defining the possible controls which the optimizer can take. In the case of a rocket, the main control is its thruster. While many thruster configurations exist for rockets, a common one to assume is a single thruster situated about the rocket’s tail end, capable of gimbaling. With this configuration, in the reference frame of a rocket’s body, the resulting control vector is formulated as

ub = ˆγΓ
, where ˆγ is the direction of the thrust in the body’s frame and Γ is the magnitude of the thrust. The controls at any moment in time are defined as the thrust magnitude Γ, inclination angle η, and azimuth angle ζ, describing the thrust vector in the body frame as
⌊ ⌋ ⌈ Γ sin(ζ)cos(η)⌉ ub = Γ sin (ζ)sin(η) Γ cos(ζ)
This leads to the definition of the control vector
⌊ ⌋ Γ A = ⌈ η ⌉ ζ

3.2.

3.2 Rigid Body

It is sufficient to model the rocket as a cylinder, where the length of the rocket is l and the radius of the rocket is r. 3.2.1.

3.2.1 Orientation

Firstly, it is necessary to establish some coordinate conventions. Firstly, the primary inertial reference frame is defined as a stationary reference frame, from which a body (i.e. the rocket) will be measured in reference to. The Z axis is defined as being directed upward, out of the ground, towards the sky. The X and Y axes are arbitrarily directed, completing the triad forming the right handed coordinate system.

The rocket’s z axis is taken to be directed from the rocket’s tail to its nose, centered about its center of mass (i.e. halfway along the rocket’s length for this discussion). Again, as with the inertial coordinate system, the rocket’s x and y axes are arbitrarily defined to complete the triad, forming the right handed coordinate system. With these conventions, rotations about the rocket’s z axis is known as rolling, and rotations about the x and y axes describe pitching and yawing (somewhat arbitrarily since the rocket is axisymmetric).

The rocket’s orientation in space with respect to the inertial frame of the simulation is robustly described using quaternions, rather than Euler angles, which often become vexatious due to their inherent propensity to encounter singularities. Taking the body’s z axis to be along its axial direction, positively away from the thruster’s location, and the x and y axes to be arbitrarily directed orthogonally to the z axis, completing the right-handed triad, the quaternion can be formulated as

⌊ qr ⌋ | qi | q = |⌈ qj |⌉ qk
, where q = (10 00)T describes the body’s orientation to be in alignment with the inertial frame. Note that, as part of the NLP constraints, qr2 = qi2 + qj2 + qk2. The resulting rotation matrix, describing the unit vectors of the body’s axes, is computed as
R = ⌊ - 2q2 - 2q2+ 1 2(qiqj - qkqr) 2 (qiqk + qjqr) ⌋ ⌈ 2(jqiqj + kqkqr) - 2q2- 2q2+ 1 2 (qjqk - qiqr) ⌉ 2(qiqk - qjqr) 2(qiiqr + kqjqk) - 2q2 - 2q2 + 1 i j
. This rotation matrix allows vectors, originally described in the body’s frame, to be represented in the inertial frame. 3.2.2.
3.2.2 Inertia

Modelling the rocket as a cylinder with length l and radius r, the three-dimensional inertia tensor can be formulated as

⌊ m-(3r2 + l2) 0 0 ⌋ ⌈ 12 0 m-(3r2 + l2) 0 ⌉ I = 12 mr2 0 0 2
. which is used in the computation of the rocket’s angular acceleration. 3.3.

3.3 Dynamics

3.3.1.

3.3.1 Control

Having constructed the robust quaternion based rotation matrix, the translational force due to the rocket’s control vector in its local frame ub can be described in the global inertial frame by premultiplying it by the aforementioned rotation matrix as follows

u = R ⋅ub
. Assuming the thruster’s location in the rocket’s coordinate system is ru = (00 - l∕2)T, the moment imparted by the control is
⌊ 6Γ lsin(ζ)sin(η) ⌋ | ---l2m+mw2--- | τ = ⌈ 6Γ lls2inm(ζ+)mcwo2s(η) ⌉ 0

3.3.2.

3.3.2 Acceleration

Having developed the model of the physical influences particular to the rocket environment, the full equation of translational motion is shown as

d2p u --2-= g + -- dt m
, where p is the position of the rocket, g = (0 0 - 9.807)T is the acceleration due to gravity, and m is the mass of the rocket governed by
dm- --Γ-- dt = - Isp ⋅g
, where Isp ≈ 311 s is the specific impulse of the rocket’s propulsion system. The angular acceleration of the rocket is as follows
dω-= τ ⋅I-1 dt
, where ω is the rocket’s angular velocity, from which the time rate of change of the quaternions, describing the rocket’s orientation, can be computed through quaternion multiplication as follows
dq 1[ 0 ] -dt = 2 ω ⊗q
3.4.

3.4 State

The state of the dynamical system, particular to the rocket, can be formalized as follows

⌊ ⌋ x || y || || z || || vx || ⌊ p ⌋ || vy || | v | || vz || S = || q || = || qr || |⌈ ω |⌉ || qi || m || qj || || qk || || ωx || || ωy || ⌈ ωz ⌉ m
with a dimensionality of fourteen.

4.

4 Results

Herein, in the analysis of this paper, the rocket’s state is initialized as

x = [100,100,100,5,5,- 5,1,0,0,0,0,0,0,10000]T
and the target’s state is initialized as
T x = [0,0,0,0,0,0,1,0,0,0,0,0,0,None]
. Hence, the rocket has a mass of 10000 kilograms. Note that the rocket is not given any target for its mass, as this is the metric to be optimized. The lower bound of the rocket’s state is initialized as
LB = [- 1000,- 1000,0,- 100,- 100,- 100, x T 0,0,0,0,- 100,- 100,- 100,0]
and the upper bound is initialized as
UB = [1000,1000,1000,100,100,100, x T 1,1,1,1,100,100,100,10000]
. In addition to the rocket’s state, bounds are also placed on the rocket’s controls. The control’s lower bound is defined as
LB = [0,0,0]T u
and upper bound as
UBu = [44000,π,2π ]T
Using the sequential quadratic programming algorithm, with five grid nodes n = 5, a final mass of mf = 9863.16 kg is achieved, as shown Figure 2. With ten grid nodes n = 10, a final mass of mf = 9902.53 kg is achieved, with a convergence seen in Figure 3. Thus, as one can see, the accuracy of both the dynamical model and optimization method are confirmed, and more accurate results will be obtained with increasing nodes. The physical characterizations of the system’s state and control for n = 5, n = 10, n = 15, and n = 25 nodes are seen in Figure 4, 5, 6, 7, respectively. The final masses achieved through this trajectory optimization process, among all node amounts, compare to that of an analogous example with equivalent metrics, using the indirect method, implemented in the documentation of the European Space Agency’s open-source optimizer, PyGMO [?]. It should be noted, however, that increasing the number of nodes beyond n = 25 proved to be incessant; even with parallel CPU core usage, computations were rather long winded do to sheer number of variables in the NLP problem.

5.

5 Conclusions

This paper demonstrated a feasible trajectory optimization methodology to replicate the soft, upright landing of a rocket, as first demonstrated by SpaceX and Blue Origin. Future improvements will include the migration to a low-level language, such as C++, to save on computational expenses. Additionally, different methods of transcription will be added to the current software library. And lastly, the software library will abstract itself to more general usage for such applications as interplanetary spacecraft trajectory optimization and quad-copters.

PIC

Fig. 2: Convergence with 5 Nodes

PIC

Fig. 3: Convergence with 10 Nodes

PIC

Fig. 4: Trajectory with 5 Nodes

PIC

Fig. 5: Trajectory with 10 Nodes

PIC

Fig. 6: Trajectory with 15 Nodes

PIC

Fig. 7: Trajectory with 25 Nodes

6.

6 Source Code


Listing 1: Rocket Class - Matlab
 
classdef Rocket 
    % This class describes a rocket, modeled as a 
    % three dimensional solid in Euclidian space. 
    % The rocket has a length (long side), and a 
    % width (or diameter), from which its inertia 
    % tensor is computed, in order to 
    properties 
        % State = [Position,Velocity,Quaternion,Euler Rates,Mass] 
        % State = [x,y,z,vx,vy,vz,qr,qi,qj,qk,wx,wy,wz,m] 
        State;    % Initial state 
        Target;   % Target state 
        Length;   % Rocket length 
        Width;    % Rocket width 
        Isp;      % Specific impulse 
        NNodes;   % Number of discritization nodes 
        StateDim; % Dimension of state 
        ContDim;  % Dimension of control 
        NAux;     % Number of auxilary variables 
        NMain;    % Number of main variables 
        NLPDim;   % Number of NPL variables 
        LB;       % Lower NLP bounds 
        UB;       % Upper NLP bounds 
    end 
 
    methods 
 
        function self = Rocket(state,target,L,W,Isp,NNodes) 
            self.State    = state; 
            self.Target   = target; 
            self.Length   = L; 
            self.Width    = W; 
            self.Isp      = Isp; 
            self.NNodes   = NNodes; 
            self.StateDim = 14; 
            self.ContDim  = 3; 
            self.NAux     = 1; % Final time 
            self.NMain    = self.StateDim+self.ContDim; 
            self.NLPDim   = self.NMain⋆self.NNodes+self.NAux; 
            self.LB       = []; 
            self.UB       = []; 
            for n=1:self.NNodes 
               % Bounds for state per node 
               lbs = [-1000;-1000;0;... 
                      -100;-100;-100;... 
                      0;0;0;0;... 
                      -100;-100;-100;... 
                      0]; 
               ubs = [1000;1000;1000;... 
                      100;100;100;... 
                      1;1;1;1;... 
                      100;100;100;... 
                      self.State(14)]; 
               % Bounds for control per node 
               lbc     = [0;0;0]; 
               ubc     = [44000;0;2⋆pi]; 
               % Concatenate 
               lb      = [lbs;lbc]; 
               ub      = [ubs;ubc]; 
               % Concatenate to master 
               self.LB = [self.LB;lb]; 
               self.UB = [self.UB;ub]; 
            end 
            % Final time bounds (auxilary) 
            self.LB = [self.LB;10]; 
            self.UB = [self.UB;7200]; % 2 hours to land 
        end 
 
        function PlotTrajectory(self,x) 
            [state,control,tf] = self.DecodeNLP(x); 
            t = linspace(0,tf,self.NNodes); 
 
            x = state(:,1); 
            y = state(:,2); 
            z = state(:,3); 
            subplot(3,3,1); 
            plot3(x,y,z); 
            title('Trajectory␣[m]','Interpreter','latex'); 
            xlabel('x','Interpreter','latex'); 
            ylabel('y','Interpreter','latex'); 
            zlabel('z','Interpreter','latex') 
            grid on 
 
            subplot(3,3,2); 
            plot(t,x,t,y,t,z); 
            title('Position␣[m]','Interpreter','latex'); 
            legend('x','y','z'); 
            legend('Location','best'); 
            legend('boxoff'); 
 
            vx = state(:,4); 
            vy = state(:,5); 
            vz = state(:,6); 
            subplot(3,3,3); 
            plot(t,vx,t,vy,t,vz); 
            title('Velocity␣[m/s]','Interpreter','latex'); 
            legend('v_x','v_y','v_z'); 
            legend('Location','best'); 
            legend('boxoff'); 
 
            qr = state(:,7); 
            qi = state(:,8); 
            qj = state(:,9); 
            qk = state(:,10); 
            subplot(3,3,4); 
            plot(t,qr,t,qi,t,qj,t,qk); 
            title('Quaternions␣[ND]','Interpreter','latex'); 
            legend('q_r','q_i','q_j','q_k'); 
            legend('Location','best'); 
            legend('boxoff'); 
 
            wx = state(:,11); 
            wy = state(:,12); 
            wz = state(:,13); 
            subplot(3,3,5); 
            plot(t,wx,t,wy,t,wz); 
            title('Angular␣Velocity␣[rad/s]','Interpreter','latex'); 
            legend('\omega_x','\omega_y','\omega_z'); 
            legend('Location','best'); 
            legend('boxoff'); 
 
            m  = state(:,14); 
            subplot(3,3,6); 
            plot(t,m); 
            title('Mass␣[kg]','Interpreter','latex'); 
 
            thrust = control(:,1); 
            subplot(3,3,7); 
            plot(t,thrust); 
            title('Thrust␣[N]','Interpreter','latex'); 
 
            incl = control(:,2); 
            subplot(3,3,8); 
            plot(t,incl); 
            title('Thrust␣Inclination␣[rad]','Interpreter','latex'); 
 
            azim = control(:,3); 
            subplot(3,3,9); 
            plot(t,azim); 
            title('Thrust␣Azimuth␣[rad]','Interpreter','latex'); 
        end 
 
 
        function [x,fval] = Optimise(self,x0,type) 
            func    = @self.Objective; 
            nvar    = self.NLPDim; 
            A       = []; 
            b       = []; 
            Aeq     = []; 
            beq     = []; 
            lb      = self.LB; 
            ub      = self.UB; 
            nonlcon = @self.Constraints; 
            if strcmp(type,'fmincon') 
                options = optimoptions(@fmincon,'Display','iter-detailed',... 
                    'PlotFcn',@optimplotfval,'UseParallel',true,... 
                    'ScaleProblem','obj-and-constr',... 
                    'Algorithm','sqp',... 
                    'MaxFunctionEvaluations',100000); 
                [x,fval] = fmincon(func,x0,A,b,Aeq,beq,lb,ub,nonlcon,options); 
            elseif strcmp(type,'genetic') 
                options = optimoptions(@ga,'PlotFcn',@gaplotbestf,... 
                    'UseParallel',true,'Display','iter',... 
                    'CreationFcn',@gacreationunlinearfeasible); 
                [x,fval] = ga(func,self.NLPDim,A,b,... 
                              Aeq,beq,lb,ub,nonlcon,options); 
            end 
        end 
 
        function mass = Objective(self,x) 
            % Decode the descision vector 
            state = self.DecodeNLP(x); 
            % Compute the final mass 
            mass  = -state(self.NNodes,14); 
        end 
 
        function [c,ceq] = Constraints(self,x) 
            % First we decode the descision vector 
            [state,control,tf] = self.DecodeNLP(x); 
            % We define the time grid 
            t   = linspace(0,tf,self.NNodes); 
            ceq = []; 
            % Trapezoidal quadrature to compute defects 
            for n=1:self.NNodes-1; 
                s1  = state(n,:).'; 
                s2  = state(n+1,:).'; 
                u1  = control(n,:).'; 
                u2  = control(n+1,:).'; 
                f1  = self.RHS(s1,u1); 
                f2  = self.RHS(s2,u2); 
                h1  = t(n+1)-t(n); 
                ceq = [ceq;s2-s1-0.5⋆h1.⋆(f1+f2)]; 
            end 
            % Satisfy quaternion norms 
            for n=1:self.NNodes 
                q   = state(n,7:10).'; 
                ceq =[ceq;q(1)^2+q(2)^+q(3)^2+q(4)^2-1]; 
            end 
            % Satisfy initial condition 
            ceq = [ceq;self.State-state(1,:).']; 
            % Satisfy terminal conditions (no mass) 
            ceq = [ceq;self.Target(1:13)-state(self.NNodes,1:13).']; 
            % No inequality constraints 
            c   = []; 
        end 
 
 
        function [s,c,tf] = DecodeNLP(self,x) 
            tf = x(self.NLPDim); % Final time (auxilary) 
            x  = x(1:self.NLPDim-1); % Without time 
            x  = reshape(x,[self.NMain,self.NNodes]).'; 
            s  = x(:,1:self.StateDim); 
            c  = x(:,self.StateDim+1:self.NMain); 
        end 
 
 
        function dS = RHS(self,state,control) 
            p    = state(1:3);   % Position [m] 
            v    = state(4:6);   % Velocity [m] 
            q    = state(7:10);  % Quaternion [ND] 
            w    = state(11:13); % Angular velocity [rad/s] 
            m    = state(14);    % Mass [kg] 
            T    = control(1);   % Thrust magnitude [N] 
            incl = control(2);   % Thrust body inclination [rad] 
            azim = control(3);   % Thrust body azimuth [rad] 
            % Thrust direction in body frame 
            gamma = [sin(incl)⋆cos(azim);sin(incl)⋆sin(azim);cos(incl)]; 
            ub    = T.⋆gamma; % Thrust vector in body frame [N] 
            u     = self.QuatVecTrans(q,ub); % Inertial thrust vector [N] 
            r     = [0;0;-self.Length/2]; % Moment arm 
            tau   = cross(r,ub); % Thrust moment [Nm] 
            % Inertia of solid 
            I     = self.Inertia(self.Width,self.Length,m); 
            g     = [0;0;-9.807]; % Gravitational [m/s^2] 
            % Assemble the state transition 
            dS(1:3,1) = v; %[m/s] 
            dS(4:6)   = g + u./m; % Translation [m/s^2] 
            dS(7:10)  = self.dqdt(q,w); % Quaternions [ND] 
            dS(11:13) = inv(I)⋆tau; % Rotational [rad/s^2] 
            dS(14)    = -T/(self.Isp⋆9.807); % Mass flow [kg/s] 
        end 
 
        function vec = QuatVecTrans(self,q,u) 
            qcon  = [q(1);-q(2);-q(3);-q(4)]; % Conjugate 
            qnorm = norm(q); % Quaternion normal 
            qinv  = qcon./qnorm; % Quaternion inverse 
            u     = [0;u]; % Make quaternion 
            u     = self.QuatMult(u,qinv); 
            u     = self.QuatMult(q,u); 
            vec   = u(2:4); 
        end 
 
        function qp = QuatMult(self,q1,q2) 
            s1 = q1(1); 
            v1 = q1(2:4); 
            s2 = q2(1); 
            v2 = q2(2:4); 
            s  = s1⋆s2 - dot(v1,v2); 
            v  = s1.⋆v2 + s2.⋆v1 + cross(v1,v2); 
            qp = [s;v]; 
        end 
 
        function I = Inertia(self,r,h,m) 
            Ixy = (m/12)⋆(3⋆r^2 + h^2); 
            Iz = (m⋆r^2)/2; 
            I  = [Ixy,0,0;0,Ixy,0;0,0,Iz]; 
        end 
 
        function dq = dqdt(self,q,w) 
            w  = [0;w]; % Make quaternion 
            dq = self.QuatMult(q./2,w); 
        end 
 
    end 
end

Listing 2: Rocket Class - Python
 
from numpy import (hsplit, array,zeros_like,insert,cross,append,radians,sin,cos,empty,pi,vstack,float64,linspace,empty,zeros,arange) 
from numpy.linalg import (norm,inv) 
from scipy.integrate import odeint 
from PyGMO.problem._base import base 
from numpy.random import randn, uniform 
import matplotlib.pyplot as plt 
import matplotlib as mpl 
from mpl_toolkits.mplot3d import Axes3D 
 
class Rocket(object): 
 
    def __init__( 
        self, 
        state0 = [0,0,1000,5,0,-5,1,0,0,0,0,0,0,10000], 
        statet = [0,0,0,0,0,0,1,0,0,0,0,0,0,None], 
        params = [5,1,2000] 
    ): 
        ''' 
        Initial State: state0 | Target State: statet 
        state = [x,y,z,vx,vy,vz,qr,qi,qj,qk,wx,wy,wz,m] 
        in [m,m,m,m/s,m/s,m/s,ND,ND,ND,ND,rad/s,rad/s,rad/s,kg] 
        Parametres: params = [L,W,Isp] in [m,m,sec] 
        Optimization Parametres = [N_Nodes] 
 
        Control Vector: 
        Control vector for the spacecraft's propulsion 
        system, control = [T,incl,azim]. T denotes the 
        magnitude to which the thruster is fired, incl 
        indicates the thrust vector's inclination angle 
        measured from the positive z-axis in the body 
        frame, and azim indicated the azimuthal angle of 
        the thrust vector measured from the positive x-axis. 
        These three spherical coordinate parametres form a 
        control vector in the body fixed frame, from which 
        the translation force vector is computed in the 
        inertial frame, determined by the body's orientation. 
 
        NLP Descision Vector: 
        x = [s1,u1,s2,u2,...,sf,uf,t0,tf] 
        x = [x1,y1,z1,vx1,vy1,vz1,qr1,qi1,qj1,qk1,... 
             wx1,wy1,wz1,m1,T1,i1,a1,x2,y2,z2,vx2,... 
             vy2,vz2,qr2,qi2,qj2,qk2,wx2,wy2,wz2,m2,... 
             T2,i2,a2,...,xf,yf,zf,vxf,vyf,vzf,qrf,... 
             qif,qjf,qkf,wxf,wyf,wzf,mf,Tf,if,af,tf] 
 
        ''' 
 
        # Initialize 
        self.State = array(state0,dtype =float64) 
        self.Target = array(statet,dtype =float64) 
        self.Params = array(params,dtype =float64) 
 
        # Bounds 
        self.StateBounds = array([ 
            [-1000,1000],[-1000,1000],[0,1000], 
            [-100,100],[-100,100],[-100,100], 
            [0,1],[0,1],[0,1],[0,1], 
            [-10,10],[-10,10],[-10,10], 
            [1000,1000] 
        ]) 
        self.ContBounds = array([[0,1765800],[0,pi],[0,2⋆pi]],dtype=float64) 
 
        # Nonlinear Programme 
        self.StateDim = 14 # State dimensions 
        self.ContDim  = 3  # Control dimensions 
        self.NNodes   = 10 # Number of nodes 
        self.NAux     = 1  # Number of auxilary variables 
        self.NMain    = self.StateDim + self.ContDim 
        self.NLPDim   = self.NMain⋆self.NNodes+self.NAux 
 
    def Optimise(self,x0): 
        cons = ({'type': 'eq', 'fun': self.Defects}) 
        return None 
 
    def Bounds(self,x): 
        print x 
 
 
    def Objective(self,x): 
        states,controls,tf = self.Decode_NLP(x) 
        obj = -states[-1,-1] # Maximize mass 
        return obj 
 
    def Constraints(self,x): 
        ''' 
        Returns scaler objective and equality constraints. 
        ''' 
        states,controls,tf = self.Decode_NLP(x) 
        t = linspace(0,tf,self.NNodes) # Time grid 
        constr = states[0]-self.State # Initial 
        print constr 
        for n in range(self.NNodes-1): 
            s1 = states[n] 
            s2 = states[n+1] 
            u1 = controls[n] 
            u2 = controls[n+1] 
            h1 = t[n+1]-t[n] 
            f1 = self.RHS(s1,u1) 
            f2 = self.RHS(s2,u2) 
            constr = append(constr,s2-s1-0.5⋆h1⋆(f1+f2)) 
        eqconstr = append(constr,states[-1]-self.Target) # Terminal 
        return eqconstr 
 
    def RHS(self,state,control): 
        ''' 
        Returns rate of change of state 
        given current state and control. 
        Control: 
        Thrust magnitude        = Tmag 
        Body thrust inclination = incl 
        Body thrust azimuth     = azim 
        ''' 
        # State 
        p = state[0:3]   # Position 
        v = state[3:6]   # Velocity 
        q = state[6:10]  # Quaternion 
        w = state[10:13] # Euler Rates 
        m = state[13]    # Mass 
        # Parametres 
        L,W,Isp = self.Params 
        # Control parametres 
        T, incl, azim = control 
        # Thrust direction in body frame 
        gamma = array([sin(incl)⋆cos(azim), 
                       sin(incl)⋆sin(azim), 
                       cos(incl)]) 
        # Thrust vector in body frame [N] 
        ub  = T⋆gamma 
        # Thrust vector in inertial frame [N] 
        u   = self.QuatVecTrans(q,ub) 
        # Moment arm of thrust [m] 
        r   = array([0,0,-L/2]) 
        # Moment due to thrust [Nm] 
        tau = cross(r,ub) 
        # Inertia tensor 
        I   = self.Cuboid_Inertia(W,W,L,m) 
        # Gravity 
        g = array([0,0,-9.807]) 
        # Assemble first order system 
        dS        = zeros_like(state) 
        dS[0:3]   = v 
        dS[3:6]   = g + u/m 
        dS[6:10]  = self.dqdt(q,w) 
        dS[10:13] = tau.dot(inv(I)) 
        dS[13]    = -T/(Isp⋆9.807) 
        return dS 
 
    def Decode_NLP(self,x): 
        ''' 
        Decodes the nonlinear programme descision 
        vector. 
        ''' 
        tf = x[-1] 
        x  = x[:-1].reshape((self.NNodes,self.NMain)) 
        states, controls = hsplit(x,[self.StateDim]) 
        return states,controls,tf 
 
    def xrand(self): 
        xrand = [] 
        for n in range(self.NNodes): 
            for s in range(self.StateDim): 
                xrand.append(uniform(⋆self.StateBounds[s])) 
            for u in range(self.ContDim): 
                xrand.append(uniform(⋆self.ContBounds[u])) 
        xrand.append(1000) 
        return array(xrand,dtype=float64) 
 
    @staticmethod 
    def Eul2Quat(psi, theta, phi, unit): 
        if unit is 'deg': 
            psi,theta,phi = radians((psi,theta,phi)) 
        else: pass 
        a, b, c = phi/2.0, theta/2.0, psi/2.0 
        sa, ca  = sin(a), cos(a) 
        sb, cb  = sin(b), cos(b) 
        sc, cc  = sin(c), cos(c) 
        qi      = sa⋆cb⋆cc - ca⋆sb⋆sc 
        qj      = ca⋆sb⋆cc + sa⋆cb⋆sc 
        qk      = ca⋆cb⋆sc - sa⋆sb⋆cc 
        qr      = ca⋆cb⋆cc + sa⋆sb⋆sc 
        return  array([qr,qi,qj,qk]) 
 
    @staticmethod 
    def Quat2Eul(qi,qj,qk,qr,unit): 
        phi   = arctan2(2⋆(qr⋆qi+qj⋆qk), 1-2⋆(qi⋆⋆2+qj⋆⋆2)) 
        theta = arcsin(2⋆(qr⋆qj-qk⋆qi)) 
        psi   = arctan2(2⋆(qr⋆qk+qi⋆qj), 1-2⋆(qj⋆⋆2+qk⋆⋆2)) 
        if unit is 'deg': return degrees((psi,theta,phi)) 
        else: return psi, theta, phi 
 
    @staticmethod 
    def Sphere2Cart(rho,theta,phi,unit='deg'): 
        if unit is 'deg': 
            theta,phi = radians((theta,phi)) 
        else: pass 
        x = rho⋆sin(theta)⋆cos(phi) 
        y = rho⋆sin(theta)⋆sin(phi) 
        z = rho⋆cos(theta) 
        return array([x,y,z]) 
 
    @staticmethod 
    def QuatVecTrans(q,v): 
        qr,qi,qk,qj = q 
        qconj = array([qr,-qi,-qj,-qk]) 
        qnorm = norm(q) 
        qinv  = qconj/qnorm 
        v     = insert(v,0,0) 
        v     = Rocket.QuatMult(v,qinv) 
        v     = Rocket.QuatMult(q,v) 
        return v[1:] 
 
    @staticmethod 
    def QuatMult(q1,q2): 
        s1 = q1[0] 
        v1 = q1[1:] 
        s2 = q2[0] 
        v2 = q2[1:] 
        s  = s1⋆s2 - v1.dot(v2) 
        v  = s1⋆v2 + s2⋆v1 + cross(v1,v2) 
        return append(s,v) 
 
    @staticmethod 
    def dqdt(q,w): 
        w = insert(w,0,0) 
        return Rocket.QuatMult(0.5⋆q,w) 
 
    @staticmethod 
    def Cuboid_Inertia(w,h,d,m): 
        Ix = (1/12.)⋆(h⋆⋆2+d⋆⋆2) 
        Iy = (1/12.)⋆(w⋆⋆2+d⋆⋆2) 
        Iz = (1/12.)⋆(w⋆⋆2+h⋆⋆2) 
        I  = array([[Ix,0,0],[0,Iy,0],[0,0,Iz]]) 
        return I 
 
    @staticmethod 
    def Quat2RotMat(q): 
        qr     = q[0] 
        qi     = q[1] 
        qj     = q[2] 
        qk     = q[3] 
        A      = empty([3,3]) 
        A[0,0] = qr⋆⋆2+qi⋆⋆2-qj⋆⋆2-qk⋆⋆2 
        A[1,0] = 2⋆(qi⋆qj+qk⋆qr) 
        A[2,0] = 2⋆(qi⋆qk-qj⋆qr) 
        A[0,1] = 2⋆(qi⋆qj-qk⋆qr) 
        A[1,1] = qr⋆⋆2-qi⋆⋆2+qj⋆⋆2-qk⋆⋆2 
        A[2,1] = 2⋆(qj⋆qk+qi⋆qr) 
        A[0,2] = 2⋆(qi⋆qk+qj⋆qr) 
        A[1,2] = 2⋆(qj⋆qk-qi⋆qr) 
        A[2,2] = qr⋆⋆2-qi⋆⋆2-qj⋆⋆2+qk⋆⋆2 
        return A 
 
if __name__ == "__main__": 
    Falcon = Rocket() 
    x = Falcon.xrand() 
    Falcon.Constraints(x)