SCP Toolbox Workshop¶

A tutorial on generating dynamically feasible trajectories reliably and efficiently

Monday, February 7, 2022

Rocky Mountain AAS GN&C Conference, Breckenridge, CO

 Part 4: Rocket-Landing Guidance 

In this part of the tutorial, you will participate in formulating and solving a Lunar rocket landing trajectory optimization problem.

The rocket has a 2-dimensional position and a rotation, and a gimbaled rocket engine. The goal is to take the rocket from an airborne state to a soft touchdown on the ground.

Figure. Illustration of the rocket soft landing problem.

Begin by defining a new TrajectoryProblem object.

Answer

pbm = TrajectoryProblem();

Rocket parameters¶

Any trajectory optimization problem begins by defining the constant parameters of the problem. For example, how much does the rocket weigh? How high up does it start?

We define these below.

Rocket dynamics¶

The partial rocket dynamics are given by the following ordinary differential equations:

\begin{align} \dot x &= v_x, \\ \dot y &= \mathrm{\color{grey}{TODO}}, \\ \dot v_x &= -\frac{T}{m}\sin(\theta+\delta), \\ \dot v_y &= \mathrm{\color{grey}{TODO}}, \\ \dot \theta &= \mathrm{\color{grey}{TODO}}, \\ \dot \omega &= -\frac{LT}{J}\sin(\delta), \\ \dot m &= -\alpha T. \end{align}

Answer

\begin{align} \dot y &= v_y, \\ \dot v_y &= \frac{T}{m}\cos(\theta+\delta)-g, \\ \dot \theta &= \omega. \end{align}

Define the state vector $x\in\mathbb R^n$, with the elements in the same order as they appear on the left-hand side of the ordinary differential equations above:

$$ x = \begin{bmatrix} \mathrm{\color{grey}{TODO}} \end{bmatrix}. $$

Answer

$$ x = \begin{bmatrix} x \\ y \\ v_x \\ v_y \\ \theta \\ \omega \\ m \end{bmatrix} $$

Define the input vector $u\in\mathbb R^m$, with the thrust coming first:

$$ u = \begin{bmatrix} \mathrm{\color{grey}{TODO}} \end{bmatrix}. $$

Answer

$$ u = \begin{bmatrix} T \\ \delta \end{bmatrix} $$

Complete the function below that evaluates the right-hand side of the dynamics ordinary differential equations above. The function signature is:

$$ \dot x = f_{rocket}(x, u), $$

where the arguments are the state and input and the output is the state time derivative.

Answer

f_rocket(x, u) = begin
    rx, ry, vx, vy, θ, ω, m = x
    T, δ = u
    return [vx; vy; -(T/m)*sin(θ+δ); (T/m)*cos(θ+δ)-g; ω; -(L*T/J)*sin(δ); -α*T]
end;

Remember from the Dubin's car example that SCP Toolbox operates on a normalized-time problem where time takes values in $[0,1]$.

We are going to solve a free-final time rocket landing problem, and this requires us to introduce a one-element parameter vector that holds the time dilation coefficient:

$$ p = \begin{bmatrix} t_f \end{bmatrix}. $$

The normalized-time dynamics are given by:

$$ \dot x = t_f f_{rocket}(x, u) \equiv f(x, u, p). $$

This allows us to write the normalized-time dynamics in Julia as follows.

Based on the above definitions of the state ($x$), the input ($u$), and the parameter ($p$), record their dimensions ($n$, $m$, and $d$, respectively).

Answer

n = 7
m = 2
d = 1

Use the SCP Toolbox API to define the state, input, and parameter dimensions in the problem.

Answer

problem_set_dims!(pbm, n, m, d)

Jacobians¶

We need to define the Jacobians of the dynamics with respect to the state, input, and parameter vectors.

Remember the definition of the Jacobian of a general function $g(z):\mathbb R^n\to\mathbb R^m$:

$$ \nabla_z g(z) = \begin{bmatrix} \frac{\partial g(z)}{\partial z_1} & \frac{\partial g(z)}{\partial z_2} & \cdots & \frac{\partial g(z)}{\partial z_n} \end{bmatrix}\in\mathbb R^{m\times n}. $$

With the above in mind, write down "in math" the Jacobian of the dynamics with respect to $x$:

$$ A(x, u, p) = \nabla_x f(x, u, p) = \begin{bmatrix} \mathrm{\color{grey}{TODO}} \end{bmatrix}\in\mathbb R^{n\times n}. $$

To help, here is what the column corresponding to the state $x_5\equiv\theta$ looks like:

$$ \begin{bmatrix} 0\\0\\- \frac{T t_f \cos{\left(\delta + \theta \right)}}{m}\\- \frac{T t_f \sin{\left(\delta + \theta \right)}}{m}\\0\\0\\0\end{bmatrix}. $$

Answer

$$ A(x, u, p) = \begin{bmatrix} 0 & 0 & t_f & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & t_f & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & - \frac{T t_f \cos{\left(\delta + \theta \right)}}{m} & 0 & \frac{T t_f \sin{\left(\delta + \theta \right)}}{m^{2}}\\0 & 0 & 0 & 0 & - \frac{T t_f \sin{\left(\delta + \theta \right)}}{m} & 0 & - \frac{T t_f \cos{\left(\delta + \theta \right)}}{m^{2}}\\0 & 0 & 0 & 0 & 0 & t_f & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0 & 0 & 0\end{bmatrix}. $$

Now implement the function in Julia.

Answer

A(x, u, p) = begin
    rx, ry, vx, vy, θ, ω, m = x
    T, δ = u
    tf, = p
    return [0 0 1 0       0       0       0
            0 0 0 1       0       0       0
            0 0 0 0 -T/m*cos(θ+δ) 0 T/m^2*sin(θ+δ)
            0 0 0 0 -T/m*sin(θ+δ) 0 -T/m^2*cos(θ+δ)
            0 0 0 0       0       1       0
            0 0 0 0       0       0       0
            0 0 0 0       0       0       0]*tf
end

Similarly, write down the Jacobian with respect to the input, $u$:

$$ B(x, u, p) = \nabla_u f(x, u, p) = \begin{bmatrix} \mathrm{\color{grey}{TODO}} \end{bmatrix}\in\mathbb R^{n\times m}. $$

Answer

$$ B(x, u, p) = \begin{bmatrix} 0 & 0\\0 & 0\\- \frac{t_f \sin{\left(\delta + \theta \right)}}{m} & - \frac{T t_f \cos{\left(\delta + \theta \right)}}{m}\\\frac{t_f \cos{\left(\delta + \theta \right)}}{m} & - \frac{T t_f \sin{\left(\delta + \theta \right)}}{m}\\0 & 0\\- \frac{L t_f \sin{\left(\delta \right)}}{J} & - \frac{L T t_f \cos{\left(\delta \right)}}{J}\\- \alpha t_f & 0 \end{bmatrix}. $$

Now implement the function in Julia.

Answer

B(x, u, p) = begin
    rx, ry, vx, vy, θ, ω, m = x
    T, δ = u
    tf, = p
    return [     0            0
                 0            0
            -sin(θ+δ)/m -T/m*cos(θ+δ)
             cos(θ+δ)/m -T/m*sin(θ+δ)
                 0            0
            -L/J*sin(δ) -L*T/J*cos(δ)
                -α            0       ]*tf
end

Finally, write down the Jacobian with respect to the parameter vector, $p$:

$$ F(x, u, p) = \nabla_p f(x, u, p) = \begin{bmatrix} \mathrm{\color{grey}{TODO}} \end{bmatrix}\in\mathbb R^{n\times d}. $$

Answer

$$ F(x, u, p) = f_{rocket}(x, u). $$

Now implement the function in Julia.

Answer

F(x, u, p) = begin
    rx, ry, vx, vy, θ, ω, m = x
    T, δ = u
    tf, = p
    return reshape(f(x, u, p)/tf, 7, 1)
end;

The SCP Toolbox API also passes other arguments to the dynamics and its Jacobians:

• t: the time;
• k: the discrete time index;
• pbm: the TrajectoryProblem object that we defined at the very beginning.

We write a simple wrapper to satisfy the API.

Use the above functions f_scp, A_scp, B_scp, F_scp in order to define the dynamics using the SCP Toolbox API.

Answer

problem_set_dynamics!(pbm, f_scp, A_scp, B_scp, F_scp)

Boundary Conditions¶

Let's define the boundary conditions on the state as a fixed "airborne" point above the landing site at the start, and a point at the landing state at the end of the trajectory. Because the rocket depletes mass, and we do not know by how much ahead of time, the final mass is left unspecified.

Recall that the boundary conditions are defined as the equality constraints:

\begin{align} g_{ic}(x(0), p) &= 0, \\ g_{tc}(x(1), p) &= 0. \end{align}

Given the definitions of $x_0$ (x_0) and $x_f$ (x_f), define the $g_{ic}$ and $g_{tc}$ functions.

\begin{align} g_{ic}(x, p) &= \mathrm{\color{grey}{TODO}}, \\ g_{tc}(x, p) &= \mathrm{\color{grey}{TODO}}. \end{align}

Answer

\begin{align} g_{ic}(x, p) &= x-x_0, \\ g_{tc}(x, p) &= E_{m} x-x_f, \end{align} where: $$ E_{m} = \begin{bmatrix} I\in\mathbb R^{6\times 6} &\mid & 0 \end{bmatrix}\in\mathbb R^{6\times 7}. $$

Now implement these functions in Julia.

Answer

g_ic(x, p) = x-x_0
g_tc(x, p) = x[1:6]-x_f;

Again, we need to provide the Jacobians of $g_{ic}$ and $g_{tc}$ for the SCP algorithm to work with. This is needed because, in general, these may be nonaffine functions. Define the following Jacobians:

\begin{align} H_0(x(0), p) &= \nabla_x g_{ic}(x(0), p), \\ K_0(x(0), p) &= \nabla_p g_{ic}(x(0), p), \\ H_f(x(1), p) &= \nabla_x g_{tc}(x(1), p), \\ K_f(x(1), p) &= \nabla_p g_{tc}(x(1), p). \end{align}

When a Jacobian is not provided, the SCP Toolbox assumes that it is zero. Write down the non-zero Jacobians for $g_{ic}$ and $g_{tc}$ (omit those that are zero).

\begin{align} H_0(x(0), p) &= \mathrm{\color{grey}{TODO}}, \\ K_0(x(0), p) &= \mathrm{\color{grey}{TODO}}, \\ H_f(x(1), p) &= \mathrm{\color{grey}{TODO}}, \\ K_f(x(1), p) &= \mathrm{\color{grey}{TODO}}. \end{align}

Answer

\begin{align} H_0(x(0), p) &= I\in\mathbb R^{7\times 7}, \\ H_f(x(1), p) &= \begin{bmatrix} I\in\mathbb R^{6\times 6} & 0\end{bmatrix}\in\mathbb R^{6\times 7}. \end{align}

Now implement the non-zero Jacobians in Julia. You can delete the lines for the zero Jacobians.

Answer

H_0(x, p) = I(7)
H_f(x, p) = collect(hcat(I(6), zeros(6)));

We now use the SCP Toolbox API in order to associate the boundary conditions with the trajectory optimization problem. The code below configures the above functions to accept the arguments that the toolbox will provide them with. The API function problem_set_bc! is then used to link the boundary conditions to the trajectory problem.

Your job is to uncomment the problem_set_bc! calls, and remove the zero Jacobians from each call.

Answer

problem_set_bc!(pbm, :ic, wrap(g_ic), wrap(H_0))
problem_set_bc!(pbm, :tc, wrap(g_tc), wrap(H_f));

State Constraints¶

In order to not collide with the ground or any obstacles (such as hills) surrounding the landing site, the rocket's position is constrained to be above a certain glideslope, as illustrated in the figure at the top of this notebook.

Write down the glideslope constraint on the position vector $[x;y]$. (Hint: it is a convex inequality and involves $|x|$).

$$ \mathrm{\color{grey}{TODO}}\le 0. $$

Answer

$$ |x|-\frac{y}{\tan(\gamma_{gs})}\le 0. $$

What kind of convex cone can represent this constraint?

Enter your answer in term of the Julia variable that is used to by the SCP Toolbox to denote the cone (recall the table from the Part 1 notebook): TODO.

Answer

L1

Now use the SCP Toolbox's @add_constraint macro (which you learned about in Part 1) in order to define the glideslope constraint. Assume that the function receives x (the full state vector) and clp (the ConicProgram object).

Answer

glideslope(x, clp) = @add_constraint(clp, L1, x -> begin
        x, y = x[1], x[2]
        return [y/tan(γ_gs); x]
    end);

Let's associate this constraint with the trajectory problem using the SCP Toolbox API for convex state constraints.

Control Constraints¶

The control constraints ensure that the rocket's propulsion system can actually reproduce the inputs that are computed by the optimal trajectory.

For this problem, we will constraint the thrust to $T\in [T_{\min}, T_{\max}]$ and the gimbal angle $\delta\in [-\delta_{\max},\delta_{\max}]$.

Write these control constraints as cones. Write the constraints separately for thrust lower bound, upper bound, and gimbal angle (thus you should end up with 3 cones). Only use the cones that are supported by the SCP Toolbox (refer to Part 1).

Answer

\begin{align*} T_{\min}-T &\in \mathbb K_{\le 0}, \\ T-T_{\max} &\in \mathbb K_{\le 0}, \\ \begin{bmatrix} \delta_{\max} \\ \delta \end{bmatrix} &\in \mathbb K_{1}. \end{align*}

Now implement each constraint in Julia using @add_constraint.

Answer

thrust_upper_bound(T, clp) = @add_constraint(clp, NONPOS, T -> T-T_max)
thrust_lower_bound(T, clp) = @add_constraint(clp, NONPOS, T -> T_min-T)
gimbal_limit(δ, clp) = @add_constraint(clp, L1, δ -> vcat(δ_max, δ));

Now call the SCP Toolbox API function problem_set_U! in order to associate the convex input constraints with the trajectory problem.

Answer

problem_set_U!(pbm, (t, k, u, p, pbm, clp) -> begin
        T, δ = u[1], u[2]
        thrust_upper_bound(T, clp)
        thrust_lower_bound(T, clp)
        gimbal_limit(δ, clp)
    end)

Objective Function¶

A common objective for rocket landing guidance is to compute a trajectory that uses the least amount of fuel. This results in a lighter vehicle design that can carry more useful scientific payload, and other beneficial tradeoffs.

Minimizing fuel use is equivalent to maxizing the final mass of the rocket (since the least amount of fuel will have then been depleted). The SCP Toolbox seeks to minimize the following objective function:

$$ J(x, u, p) = \phi(x(1), p) + \int_0^1 \Gamma(x(t), u(t), p) dt. $$

Define $\phi$ and $\Gamma$ for maximizing the final mass:

\begin{align*} \phi(x(1), p) &= \mathrm{\color{grey}{TODO}}, \\ \Gamma(x(t), u(t), p) &= \mathrm{\color{grey}{TODO}}. \end{align*}

Be sure to normalize the functions, meaning that you should multiply them by a constant coefficient such that along any reasonable trajectory the following (approximately) hold: $\phi(\cdot)\in [0,1]$ and $\Gamma(\cdot)\in [0,1]$.

(Hint: one of the functions may have to be zero!)

Answer

\begin{align*} \phi(x(1), p) &= -\frac{ m(1) }{ m_{wet} }, \\ \Gamma(x(t), u(t), p) &= 0. \end{align*}

Now implement the functions in Julia. (Hint: you can delete any function that is zero).

Answer

ϕ(x, p) = -x[end]/m_wet;

We now have to pass the above functions to the objective function definition of the trajectory problem. The SCP Toolbox allows us to do this via the API functions problem_set_terminal_cost! and problem_set_running_cost!. If either one is missing, you can omit the call and it will be assumed that the corresponding part of the objective is zero.

In the cell below, uncomment the lines corresponding to the non-zero components of the cost.

Answer

problem_set_terminal_cost!(pbm, (x, p, pbm) -> ϕ(x, p));

Initial Trajectory Guess¶

The initial trajectory guess is required to get an SCP algorithm started. This guess can be quite coarse, and does not need to be feasible with respect to the dynamics and nonconvex constraints. Some SCP algorithms require the guess to be feasible with respect to the convex constraints, which is usually easy to do (in fact, it can be done automatically via a convex projection).

A straight-line guess is sufficient for the rocket landing problem. The following code interpolates from the initial to the final condition. The duration is set to the time it would take an average thrust to slow the rocket down to a hover. The angular rate is set to a constant value that rotates the rocket from its initial tilt to the final upright orientation.

Let's guess the input to be a constant thrust of T_guess amount, and zero gimbal angle. Complete the following function using straightline_interpolate.

Answer

input_guess(N) = straightline_interpolate([T_guess; 0.0], [T_guess; 0.0], N);

The following call the problem_set_guess! lets the SCP Toolbox know to use these initial guesses for the SCP solution.

Variable Scaling¶

Variable scaling is important when the solution variables are of widely different magnitudes. This is definitely the case for the rocket landing problem. For example, the gimbal angle varies in the $[-\delta_{\max}, \delta_{\max}]$ (which is on the order of $0.1$) while thrust can be as high as 100 kilonewtons and mass can be as high as several thousan kilograms.

The SCP Toolbox performs automatic scaling for variables whose magnitudes are naturally constrained by the convex constraints. However, you can also set the variable scaling manually by providing the expected value ranges for each variable. This is done using the SCP Toolbox API function problem_advise_scale!!.

The function accepts the element index in the state, input, or parameter vector, and a tuple (min, max) range for the expected values of the corresponding variable. The bounds don't have to be exact, but a well-chosen set of tight bounds will do better.

In the cell directly below, define the ranges (as (min, max) tuples) for the mass and thrust.

Answer

mass_range = (m_wet+Δm_guess, m_wet)
thrust_range = (T_min, T_max)

Solve the Problem Using PTR¶

With the trajectory problem fully defined, it's time to select an SCP algorithm to solve it. This comes down to selecting the set of parameters that guide the algorithm's behavior.

The toolbox currently provides three algorithsm: PTR, SCvx, and GuSTO. The latter two are discussed in detail in the IEEE Control Systems Magazine tutorial paper. The first algorithm (which goes by the full name of "Penalized Trust Region") is described in several papers, most notably an AIAA Journal of Guidance, Control, and Dynamics paper on rocket landing.

This time we will use PTR to solve the trajectory problem. The parameters below were found to work well. SCP algorithms do not have a generic set of parameters that work well for every problem. Generally, some tuning is required, but afterwards the same set of parameters applies to a wide range of perturbations of the original problem (such as in initial condition, mass, inertia, thrust magnitudes, etc.).

We can then simply "create" the SCP problem, which is a combination of the trajectory problem and the algorithm that solves it.

We subsequently call the solve method. This prints a progress table at each iteration of the SCP algorithm.

Plots¶