Added Quadcopter Problem to Documentation

docs/make.jl

@@ -27,6 +27,7 @@ makedocs(;
         "API Documentation" => "documentation/api_docs.md",
         "Tutorials" => [
             "Supply Chain Optimization" => "tutorials/supply_chain.md",
+            "Optimal Control of a Quadcopter" => "tutorials/quadcopter.md",
             "Optimal Control of a Natural Gas Network" => "tutorials/gas_pipeline.md"
         ],
     ],

docs/src/tutorials/quadcopter.md

@@ -0,0 +1,256 @@
+# Optimal Control of a Quadcopter
+
+By: Rishi Mandyam
+
+This tutorial notebook is an introduction to the graph-based modeling framework 
+Plasmo.jl (Platform for Scalable Modeling and Optimization) for JuMP 
+(Julia for Mathematical Programming).
+
+The following problem comes from the paper of Na, Shin,  Anitescu, and Zavala (available [here](https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=9840913)).
+
+A quadcopter operates in 3-D space with positions $(x, y, z)$ and angles ($\gamma$, $\beta$, and $\alpha$). 
+$g$ is the graviational constant. The set of state variables at time $t$ are treated as $\boldsymbol{x}_t = (x, y, z, \dot{x}, \dot{y}, \dot{z}, \gamma, \beta, \alpha)$. 
+The input variables at time $t$ are $\boldsymbol{u}_t = (a, \omega_x, \omega_y, \omega_z)$. 
+
+The quadcopter control problem can be written as an optimization problem as:
+
+```math
+\begin{align*}
+    \min &\; \phi(t) := \int_{0}^T \frac{1}{2} (\boldsymbol{x}(t) - \boldsymbol{x}(t)^{ref})^\top Q (\boldsymbol{x}(t) - \boldsymbol{x}(t)^{ref}) + \boldsymbol{u}(t)^\top R \boldsymbol{u}(t) dt \\
+    \textrm{s.t.} &\; \frac{d^2x}{dt^2} = a (\cos(\gamma) \sin( \beta) \cos (\alpha)  + \sin (\gamma) \sin (\alpha)) \\
+&\; \frac{d^2 y}{dt^2} = a (\cos (\gamma) \sin (\beta) \sin (\alpha) - \sin (\gamma) \cos (\alpha)) \\
+&\; \frac{d^2 z}{dt^2} = a \cos (\gamma) \cos (\beta) - g \\
+&\; \frac{d\gamma}{dt} = (\omega_x \cos (\gamma) + \omega_y \sin (\gamma)) / \cos (\beta)\\
+&\;\frac{d\beta}{dt} = -\omega_x \sin (\gamma) + \omega_y \cos (\gamma) \\
+&\;\frac{d\alpha}{dt} = \omega_x \cos (\gamma) \tan (\beta) + \omega_y \sin (\gamma) \tan (\beta) + \omega_z
+\end{align*}
+```
+
+We will model this problem in Plasmo by discretizing the problem into finite time points and representing each time point with a node. 
+
+### 1. Import Packages
+To begin, we will import and use the necessary packages
+
+```julia 
+using JuMP
+using Plasmo
+using Ipopt
+using Plots
+using LinearAlgebra
+```
+
+
+### 2. Function Design
+
+We will define a function called `Quad` that will take arguments for the number of nodes and the discretization size (i.e., $\Delta t$), optimize the model, and return the graph and reference values $x^{ref}$. 
+
+The function inputs are:
+- number of nodes (`N`)
+- time discretization (number of seconds between nodes `dt`)
+
+The function outputs are:
+- The graph
+- The objective value of the discretized form of $\phi$
+- An array with the reference values on each node ($x^{ref}$)
+
+We first establish the set points for each timepoint. The quadcopter will fly in a linear upward path in the positive X, Y, and Z directions.
+```julia
+function Quad(N, dt)
+    X_ref = 0:10/N:10;
+    dXdt_ref = zeros(N);
+    Y_ref = 0:10/N:10;
+    dYdt_ref = zeros(N)
+    Z_ref = 0:10/N:10;
+    dZdt_ref = zeros(N);
+    g_ref = zeros(N);
+    b_ref = zeros(N);
+    a_ref = zeros(N);
+```    
+
+We next combine these vectors into another vector which we call $x^{ref}$, and then define the necessary constants and arrays.
+
+```julia
+    xk_ref = [X_ref, dXdt_ref, Y_ref, dYdt_ref, Z_ref, dZdt_ref, g_ref, b_ref, a_ref];
+    grav = 9.8 # m/s^2
+
+    Q = diagm([1, 0, 1, 0, 1, 0, 1, 1, 1]);
+    R = diagm([1/10, 1/10, 1/10, 1/10]);
+
+    xk_ref1 = zeros(N,9)
+    for i in (1:N)
+        for j in 1:length(xk_ref)
+            xk_ref1[i,j] = xk_ref[j][i]
+        end
+    end
+```
+
+We can now begin building the OptiGraph. We now initialize an OptiGraph and set the optimizer. We then define `N` nodes on the OptiGraph `graph`. 
+```Julia
+    graph = OptiGraph()
+    solver = optimizer_with_attributes(Ipopt.Optimizer, "max_iter" => 100)
+    set_optimizer(graph, solver)
+    @optinode(graph, nodes[1:N])
+```
+
+Next, we loop through the nodes, and define variables on each node. These variables include dummy variables for the second derivatives and the angles' derivatives to avoid nonlinearities in the linking constraints. We also define the objective function and define arrays for the variables (to simplify the objective function formulation). 
+```julia        
+    for (i, node) in enumerate(nodes)
+
+        # Create state variables
+        @variable(node, g)
+        @variable(node, b)
+        @variable(node, a)
+
+        @variable(node, X)
+        @variable(node, Y)
+        @variable(node, Z)
+
+        @variable(node, dXdt)
+        @variable(node, dYdt)
+        @variable(node, dZdt)
+
+        # Create input variables
+
+        @variable(node, C_a)
+        @variable(node, wx)
+        @variable(node, wy)
+        @variable(node, wz)
+
+        # These expressions to simplify the linking constraints later
+        @expression(node, d2Xdt2, C_a * (cos(g) * sin(b) * cos(a) + sin(g) * sin(a)))
+        @expression(node, d2Ydt2, C_a * (cos(g) * sin(b) * sin(a) + sin(g) * cos(a)))
+        @expression(node, d2Zdt2, C_a * cos(g) * cos(b) - grav)
+
+        @expression(node, dgdt, (wx * cos(g) + wy * sin(g)) / (cos(b)))
+        @expression(node, dbdt, - wx * sin(g) + wy * cos(g))
+        @expression(node, dadt, wx * cos(g) * tan(b) + wy * sin(g) * tan(b) + wz)
+
+        xk = [X, dXdt, Y, dYdt, Z, dZdt, g, b, a] # Array to hold variables
+        xk1 = xk .- xk_ref1[i,:] # Array to hold the difference between variable values and their setpoints.
+
+        uk = [C_a, wx, wy, wz]
+        @objective(node, Min, (1 / 2 * (xk1') * Q * (xk1) + 1 / 2 * (uk') * R * (uk)) * dt)
+    end
+```
+We also set initial conditions on the first variables (for the differential equations). Note that the variables can be referenced by calling `nodes[1][<variable_reference>]`
+
+```julia
+    @constraint(nodes[1], nodes[1][:X] == 0)
+    @constraint(nodes[1], nodes[1][:Y] == 0)
+    @constraint(nodes[1], nodes[1][:Z] == 0)
+    @constraint(nodes[1], nodes[1][:dXdt] == 0)
+    @constraint(nodes[1], nodes[1][:dYdt] == 0)
+    @constraint(nodes[1], nodes[1][:dZdt] == 0)
+    @constraint(nodes[1], nodes[1][:g] == 0)
+    @constraint(nodes[1], nodes[1][:b] == 0)
+    @constraint(nodes[1], nodes[1][:a] == 0)
+```            
+
+With the variables defined, we can now add linking constraints between each time point. Note that expressions like `d2Xdt2` are the right hand side of the constraints for the derivative of `dXdt`.
+
+```julia
+    for i in 1:(N-1) # iterate through each node except the last
+        @linkconstraint(graph, nodes[i+1][:dXdt] == dt*nodes[i][:d2Xdt2] + nodes[i][:dXdt])
+        @linkconstraint(graph, nodes[i+1][:dYdt] == dt*nodes[i][:d2Ydt2] + nodes[i][:dYdt])
+        @linkconstraint(graph, nodes[i+1][:dZdt] == dt*nodes[i][:d2Zdt2] + nodes[i][:dZdt])
+
+        @linkconstraint(graph, nodes[i+1][:g] == dt*nodes[i][:dgdt] + nodes[i][:g])
+        @linkconstraint(graph, nodes[i+1][:b] == dt*nodes[i][:dbdt] + nodes[i][:b])
+        @linkconstraint(graph, nodes[i+1][:a] == dt*nodes[i][:dadt] + nodes[i][:a])
+
+        @linkconstraint(graph, nodes[i+1][:X] == dt*nodes[i][:dXdt] + nodes[i][:X])
+        @linkconstraint(graph, nodes[i+1][:Y] == dt*nodes[i][:dYdt] + nodes[i][:Y])
+        @linkconstraint(graph, nodes[i+1][:Z] == dt*nodes[i][:dZdt] + nodes[i][:Z])
+    end
+```
+
+We now have all the constraints and variables defined and can call `optimize!` on the graph. 
+
+```julia
+    set_to_node_objectives(graph)
+
+    optimize!(graph);
+
+    return objective_value(graph), graph, xk_ref
+end
+```
+Now that we have created our function to model the behavior of the quadcopter,
+we can test it using some example cases.
+
+### Examples
+First, we will run an example with 50 time points (each represented by a node) with a time discretization size of 0.1 seconds
+
+```julia
+N = 50
+dt = 0.1
+objv, graph, xk_ref = Quad(N, dt)
+nodes = getnodes(graph)
+# create empty arrays
+CAval_array = zeros(length(nodes))
+xval_array = zeros(length(nodes))
+yval_array = zeros(length(nodes))
+zval_array = zeros(length(nodes))
+
+# add values to arrays
+for (i, node)  in enumerate(nodes)
+    CAval_array[i] = value(node[:C_a])
+    xval_array[i] = value(node[:X])
+    yval_array[i] = value(node[:Y])
+    zval_array[i] = value(node[:Z])
+end
+
+xarray = Array{Array}(undef, 2)
+xarray[1] = xval_array
+xarray[2] = 0:10/(N-1):10
+
+yarray = Array{Array}(undef, 2)
+yarray[1] = yval_array
+yarray[2] = 0:10/(N-1):10
+
+zarray = Array{Array}(undef, 2)
+zarray[1] = zval_array
+zarray[2] = 0:10/(N-1):10
+```
+Now, let's visualize the position of the quadcopter in relation to its setpoint in each dimension. Below is the code for doing so in the x-dimension, and the code can be adapted for the y and z dimensions. 
+
+```julia
+plot((1:length(xval_array)), xarray[1:end], 
+  title = "X value over time", 
+  xlabel = "Node (N)", 
+  ylabel = "X Value", 
+  label = ["Current X position" "X Setpoint"]
+)
+```
+
+<img src = "../assets/Quadcopter_Xpos.png" alt = "drawing" width = "600"/>
+<img src = "../assets/Quadcopter_Ypos.png" alt = "drawing" width = "600"/>
+<img src = "../assets/Quadcopter_Zpos.png" alt = "drawing" width = "600"/>
+
+Now that we have solved for the optimal solution, let's explore a correlation. Let's see how increasing the number of nodes changes the objective value of the system. In the code snippet below, we keep the time horizon the same (10 seconds) while changing the number of nodes (i.e., the discretization intervals)
+```julia
+time_steps = 2:4:50
+
+N = length(time_steps)
+dt = .5
+obj_val_N = zeros(N)
+
+
+for i in 1:length(time_steps)
+    timing = @elapsed begin
+    objval, graph, xk_ref = Quad(time_steps[i], 10 / time_steps[i]);
+    obj_val_N[i] = objval
+    end
+    println("Done with iteration $i after ", timing, " seconds")
+end
+
+Quad_Obj_NN = plot(time_steps, obj_val_N, 
+    title = "Objective Value vs Number of Nodes (N)", 
+    xlabel = "Number of Nodes (N)", 
+    ylabel = "Objective Value",
+    label  = "Objective Value")
+```
+
+<img src = "../assets/Quadcopter_Obj_NN.png" alt = "drawing" width = "600"/>
+
+
+