├── .gitignore ├── LICENSE ├── Lecture 1 ├── Intro.pdf └── Lecture 1.pdf ├── Lecture 10 ├── Lecture 10.pdf ├── Manifest.toml ├── Project.toml └── acrobot-ilqr.ipynb ├── Lecture 11 ├── Lecture 11.pdf ├── Manifest.toml ├── Project.toml ├── acrobot.ipynb ├── cartpole.ipynb └── cartpole2.ipynb ├── Lecture 12 ├── Lecture 12.pdf ├── Manifest.toml ├── Project.toml └── dircol.ipynb ├── Lecture 13 └── Lecture 13.pdf ├── Lecture 14 ├── Lecture 14.pdf ├── Manifest.toml ├── Project.toml └── wahba.ipynb ├── Lecture 15 ├── Lecture 15.pdf ├── Manifest.toml ├── Project.toml └── quadrotor.ipynb ├── Lecture 16 ├── Lecture 16.pdf ├── Manifest.toml ├── Project.toml ├── hopper.ipynb └── hybrid-ball.ipynb ├── Lecture 17 └── Lecture 17.pdf ├── Lecture 18 ├── Lecture 18.pdf ├── Manifest.toml ├── Project.toml └── cartpole-ilc.ipynb ├── Lecture 19 ├── Lecture 19.pdf ├── Manifest.toml ├── Project.toml └── lqg.ipynb ├── Lecture 2 ├── Lecture 2.pdf ├── Manifest.toml ├── Project.toml └── integrators.ipynb ├── Lecture 20 └── Lecture 20.pdf ├── Lecture 21 └── Lecture 21.pdf ├── Lecture 22 └── Lecture 22.pdf ├── Lecture 23 └── Lecture 23.pdf ├── Lecture 24 ├── Lecture 24.pdf ├── gradient-descent-lqr-quadrotor.ipynb ├── policy-gradient-lqr-quadrotor.ipynb └── policy-gradient-lqr.ipynb ├── Lecture 3 ├── Lecture 3.pdf ├── Manifest.toml ├── Project.toml ├── minimization.ipynb └── root-finding.ipynb ├── Lecture 4 ├── Lecture 4.pdf ├── Manifest.toml ├── Project.toml ├── equality-constraints.ipynb └── minimization.ipynb ├── Lecture 5 ├── Lecture 5.pdf ├── Manifest.toml ├── Project.toml ├── inequality-constraints.ipynb ├── merit-functions.ipynb └── regularization.ipynb ├── Lecture 6 ├── Control History.pdf └── Lecture 6.pdf ├── Lecture 7 ├── Lecture 7.pdf ├── Manifest.toml ├── Project.toml ├── lqr-qp.ipynb ├── lqr-riccati.ipynb └── lqr-shooting.ipynb ├── Lecture 8 ├── Lecture 8.pdf ├── Manifest.toml ├── Project.toml └── lqr-dp.ipynb ├── Lecture 9 ├── Lecture 9.pdf ├── Manifest.toml ├── Project.toml ├── mpc.ipynb └── quadrotor_scaled.obj ├── Manifest.toml ├── Project.toml ├── README.md └── misc └── AL_tutorial.pdf /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | .DS_Store 3 | 4 | Lecture 2/.ipynb_checkpoints/ 5 | 6 | Lecture 3/.ipynb_checkpoints/ 7 | 8 | Lecture 4/.ipynb_checkpoints/ 9 | 10 | Lecture 5/.ipynb_checkpoints/ 11 | 12 | Lecture 7/.ipynb_checkpoints/ 13 | 14 | Lecture 8/.ipynb_checkpoints/ 15 | 16 | Lecture 9/.ipynb_checkpoints/ 17 | 18 | Lecture 10/.ipynb_checkpoints/ 19 | 20 | Lecture 11/.ipynb_checkpoints/ 21 | 22 | Lecture 12/.ipynb_checkpoints/ 23 | 24 | Lecture 14/.ipynb_checkpoints/ 25 | 26 | Lecture 15/.ipynb_checkpoints/ 27 | 28 | Lecture 16/.ipynb_checkpoints/ 29 | 30 | Lecture 17/.ipynb_checkpoints/ 31 | 32 | Lecture 19/.ipynb_checkpoints/ 33 | 34 | Lecture 24/.ipynb_checkpoints/ 35 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | This work is licensed under Attribution-NonCommercial-ShareAlike 4.0 International. To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-sa/4.0/ -------------------------------------------------------------------------------- /Lecture 1/Intro.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 1/Intro.pdf -------------------------------------------------------------------------------- /Lecture 1/Lecture 1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 1/Lecture 1.pdf -------------------------------------------------------------------------------- /Lecture 10/Lecture 10.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 10/Lecture 10.pdf -------------------------------------------------------------------------------- /Lecture 10/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | [[ArgTools]] 4 | uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f" 5 | 6 | [[Artifacts]] 7 | uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33" 8 | 9 | [[Base64]] 10 | uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" 11 | 12 | [[Dates]] 13 | deps = ["Printf"] 14 | uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" 15 | 16 | [[DocStringExtensions]] 17 | deps = ["LibGit2"] 18 | git-tree-sha1 = "b19534d1895d702889b219c382a6e18010797f0b" 19 | uuid = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" 20 | version = "0.8.6" 21 | 22 | [[Downloads]] 23 | deps = ["ArgTools", "LibCURL", "NetworkOptions"] 24 | uuid = "f43a241f-c20a-4ad4-852c-f6b1247861c6" 25 | 26 | [[FileIO]] 27 | deps = ["Pkg", "Requires", "UUIDs"] 28 | git-tree-sha1 = "80ced645013a5dbdc52cf70329399c35ce007fae" 29 | uuid = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549" 30 | version = "1.13.0" 31 | 32 | [[InteractiveUtils]] 33 | deps = ["Markdown"] 34 | uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" 35 | 36 | [[JLD2]] 37 | deps = ["FileIO", "MacroTools", "Mmap", "OrderedCollections", "Pkg", "Printf", "Reexport", "TranscodingStreams", "UUIDs"] 38 | git-tree-sha1 = "28b114b3279cdbac9a61c57b3e6548a572142b34" 39 | uuid = "033835bb-8acc-5ee8-8aae-3f567f8a3819" 40 | version = "0.4.21" 41 | 42 | [[LibCURL]] 43 | deps = ["LibCURL_jll", "MozillaCACerts_jll"] 44 | uuid = "b27032c2-a3e7-50c8-80cd-2d36dbcbfd21" 45 | 46 | [[LibCURL_jll]] 47 | deps = ["Artifacts", "LibSSH2_jll", "Libdl", "MbedTLS_jll", "Zlib_jll", "nghttp2_jll"] 48 | uuid = "deac9b47-8bc7-5906-a0fe-35ac56dc84c0" 49 | 50 | [[LibGit2]] 51 | deps = ["Base64", "NetworkOptions", "Printf", "SHA"] 52 | uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" 53 | 54 | [[LibSSH2_jll]] 55 | deps = ["Artifacts", "Libdl", "MbedTLS_jll"] 56 | uuid = "29816b5a-b9ab-546f-933c-edad1886dfa8" 57 | 58 | [[Libdl]] 59 | uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" 60 | 61 | [[LinearAlgebra]] 62 | deps = ["Libdl"] 63 | uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 64 | 65 | [[Logging]] 66 | uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" 67 | 68 | [[MacroTools]] 69 | deps = ["Markdown", "Random"] 70 | git-tree-sha1 = "3d3e902b31198a27340d0bf00d6ac452866021cf" 71 | uuid = "1914dd2f-81c6-5fcd-8719-6d5c9610ff09" 72 | version = "0.5.9" 73 | 74 | [[Markdown]] 75 | deps = ["Base64"] 76 | uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" 77 | 78 | [[MatrixCalculus]] 79 | deps = ["DocStringExtensions", "LinearAlgebra", "SparseArrays"] 80 | git-tree-sha1 = "7eef41327ca82345b25a3692dcd6da398a92e85f" 81 | repo-rev = "master" 82 | repo-url = "https://github.com/RoboticExplorationLab/MatrixCalculus.jl" 83 | uuid = "fafa779c-62f8-11e9-0c41-4d0261472a5f" 84 | version = "0.1.0" 85 | 86 | [[MbedTLS_jll]] 87 | deps = ["Artifacts", "Libdl"] 88 | uuid = "c8ffd9c3-330d-5841-b78e-0817d7145fa1" 89 | 90 | [[Mmap]] 91 | uuid = "a63ad114-7e13-5084-954f-fe012c677804" 92 | 93 | [[MozillaCACerts_jll]] 94 | uuid = "14a3606d-f60d-562e-9121-12d972cd8159" 95 | 96 | [[NetworkOptions]] 97 | uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908" 98 | 99 | [[OrderedCollections]] 100 | git-tree-sha1 = "85f8e6578bf1f9ee0d11e7bb1b1456435479d47c" 101 | uuid = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" 102 | version = "1.4.1" 103 | 104 | [[Pkg]] 105 | deps = ["Artifacts", "Dates", "Downloads", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "Serialization", "TOML", "Tar", "UUIDs", "p7zip_jll"] 106 | uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" 107 | 108 | [[Printf]] 109 | deps = ["Unicode"] 110 | uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" 111 | 112 | [[REPL]] 113 | deps = ["InteractiveUtils", "Markdown", "Sockets", "Unicode"] 114 | uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" 115 | 116 | [[Random]] 117 | deps = ["Serialization"] 118 | uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" 119 | 120 | [[Reexport]] 121 | git-tree-sha1 = "45e428421666073eab6f2da5c9d310d99bb12f9b" 122 | uuid = "189a3867-3050-52da-a836-e630ba90ab69" 123 | version = "1.2.2" 124 | 125 | [[Requires]] 126 | deps = ["UUIDs"] 127 | git-tree-sha1 = "838a3a4188e2ded87a4f9f184b4b0d78a1e91cb7" 128 | uuid = "ae029012-a4dd-5104-9daa-d747884805df" 129 | version = "1.3.0" 130 | 131 | [[SHA]] 132 | uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" 133 | 134 | [[Serialization]] 135 | uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" 136 | 137 | [[Sockets]] 138 | uuid = "6462fe0b-24de-5631-8697-dd941f90decc" 139 | 140 | [[SparseArrays]] 141 | deps = ["LinearAlgebra", "Random"] 142 | uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" 143 | 144 | [[TOML]] 145 | deps = ["Dates"] 146 | uuid = "fa267f1f-6049-4f14-aa54-33bafae1ed76" 147 | 148 | [[Tar]] 149 | deps = ["ArgTools", "SHA"] 150 | uuid = "a4e569a6-e804-4fa4-b0f3-eef7a1d5b13e" 151 | 152 | [[Test]] 153 | deps = ["InteractiveUtils", "Logging", "Random", "Serialization"] 154 | uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40" 155 | 156 | [[TranscodingStreams]] 157 | deps = ["Random", "Test"] 158 | git-tree-sha1 = "216b95ea110b5972db65aa90f88d8d89dcb8851c" 159 | uuid = "3bb67fe8-82b1-5028-8e26-92a6c54297fa" 160 | version = "0.9.6" 161 | 162 | [[UUIDs]] 163 | deps = ["Random", "SHA"] 164 | uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" 165 | 166 | [[Unicode]] 167 | uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" 168 | 169 | [[Zlib_jll]] 170 | deps = ["Libdl"] 171 | uuid = "83775a58-1f1d-513f-b197-d71354ab007a" 172 | 173 | [[nghttp2_jll]] 174 | deps = ["Artifacts", "Libdl"] 175 | uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d" 176 | 177 | [[p7zip_jll]] 178 | deps = ["Artifacts", "Libdl"] 179 | uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0" 180 | -------------------------------------------------------------------------------- /Lecture 10/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819" 3 | MatrixCalculus = "fafa779c-62f8-11e9-0c41-4d0261472a5f" 4 | -------------------------------------------------------------------------------- /Lecture 11/Lecture 11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 11/Lecture 11.pdf -------------------------------------------------------------------------------- /Lecture 11/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 11/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 11/Project.toml -------------------------------------------------------------------------------- /Lecture 11/cartpole2.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 16, 6 | "id": "1760edc4-4d81-4141-ba5d-5ecd70a8acaf", 7 | "metadata": {}, 8 | "outputs": [ 9 | { 10 | "name": "stderr", 11 | "output_type": "stream", 12 | "text": [ 13 | "\u001b[32m\u001b[1m Activating\u001b[22m\u001b[39m environment at `~/GitHub/lecture-notebooks/Lecture 11/Project.toml`\n" 14 | ] 15 | } 16 | ], 17 | "source": [ 18 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 19 | ] 20 | }, 21 | { 22 | "cell_type": "code", 23 | "execution_count": 17, 24 | "id": "b71c04f7-d9bf-45eb-9cd8-5ae4743596a8", 25 | "metadata": {}, 26 | "outputs": [], 27 | "source": [ 28 | "using LinearAlgebra\n", 29 | "using StaticArrays\n", 30 | "using RobotDynamics\n", 31 | "import RobotZoo.Cartpole\n", 32 | "using TrajectoryOptimization\n", 33 | "using Altro" 34 | ] 35 | }, 36 | { 37 | "cell_type": "code", 38 | "execution_count": 18, 39 | "id": "b824bb4f-09c7-4a97-96d4-e60ca9df7fb0", 40 | "metadata": {}, 41 | "outputs": [], 42 | "source": [ 43 | "model = Cartpole()\n", 44 | "n,m = size(model);" 45 | ] 46 | }, 47 | { 48 | "cell_type": "code", 49 | "execution_count": 19, 50 | "id": "cdfcb06f-cd68-4b77-be52-690cb26fb8b3", 51 | "metadata": {}, 52 | "outputs": [ 53 | { 54 | "data": { 55 | "text/plain": [ 56 | "0.05" 57 | ] 58 | }, 59 | "execution_count": 19, 60 | "metadata": {}, 61 | "output_type": "execute_result" 62 | } 63 | ], 64 | "source": [ 65 | "N = 101\n", 66 | "Tf = 5.0\n", 67 | "h = Tf/(N-1)" 68 | ] 69 | }, 70 | { 71 | "cell_type": "code", 72 | "execution_count": 20, 73 | "id": "e8ff80ef-4106-47d3-8d88-f5e2f17a8a86", 74 | "metadata": {}, 75 | "outputs": [], 76 | "source": [ 77 | "x0 = @SVector zeros(n)\n", 78 | "xf = @SVector [0, pi, 0, 0]; # i.e. swing up" 79 | ] 80 | }, 81 | { 82 | "cell_type": "code", 83 | "execution_count": 21, 84 | "id": "638586bc-4956-4a6a-bfbf-dec5d036cc93", 85 | "metadata": {}, 86 | "outputs": [], 87 | "source": [ 88 | "# Set up\n", 89 | "Q = 1.0*Diagonal(@SVector ones(n))\n", 90 | "Qf = 100.0*Diagonal(@SVector ones(n))\n", 91 | "R = 0.1*Diagonal(@SVector ones(m))\n", 92 | "obj = LQRObjective(Q,R,Qf,xf,N);" 93 | ] 94 | }, 95 | { 96 | "cell_type": "code", 97 | "execution_count": 25, 98 | "id": "d91559d8-a009-4cec-bc5c-c932b965a494", 99 | "metadata": {}, 100 | "outputs": [], 101 | "source": [ 102 | "# Create Empty ConstraintList\n", 103 | "conSet = ConstraintList(n,m,N)\n", 104 | "\n", 105 | "# Control Bounds\n", 106 | "u_bnd = 3.0\n", 107 | "bnd = BoundConstraint(n,m, u_min=-u_bnd, u_max=u_bnd)\n", 108 | "add_constraint!(conSet, bnd, 1:N-1)\n", 109 | "\n", 110 | "# Goal Constraint\n", 111 | "goal = GoalConstraint(xf)\n", 112 | "add_constraint!(conSet, goal, N)" 113 | ] 114 | }, 115 | { 116 | "cell_type": "code", 117 | "execution_count": 26, 118 | "id": "d6816995-0d1e-421d-a785-9ce87eae5a04", 119 | "metadata": {}, 120 | "outputs": [], 121 | "source": [ 122 | "prob = Problem(model, obj, xf, Tf, x0=x0, constraints=conSet);" 123 | ] 124 | }, 125 | { 126 | "cell_type": "code", 127 | "execution_count": 27, 128 | "id": "17f9edf7-14db-4ca0-ae42-5c374659040c", 129 | "metadata": {}, 130 | "outputs": [], 131 | "source": [ 132 | "u0 = @SVector fill(0.001,m)\n", 133 | "U0 = [u0 for k = 1:N-1]\n", 134 | "initial_controls!(prob, U0)\n", 135 | "rollout!(prob);" 136 | ] 137 | }, 138 | { 139 | "cell_type": "code", 140 | "execution_count": 28, 141 | "id": "cd218999-1097-4b4f-9a4b-678c37af4e07", 142 | "metadata": {}, 143 | "outputs": [ 144 | { 145 | "name": "stdout", 146 | "output_type": "stream", 147 | "text": [ 148 | "\u001b[32;1m\n", 149 | "SOLVE COMPLETED\n", 150 | "\u001b[0m solved using the \u001b[0m\u001b[36;1mALTRO\u001b[0m Solver,\n", 151 | " part of the Altro.jl package developed by the REx Lab at Stanford and Carnegie Mellon Universities\n", 152 | "\u001b[34;1m\n", 153 | " Solve Statistics\n", 154 | "\u001b[0m Total Iterations: 97\n", 155 | "\u001b[0m Solve Time: 19.242791999999998 (ms)\n", 156 | "\u001b[34;1m\n", 157 | " Covergence\n", 158 | "\u001b[0m Terminal Cost: 54.73317974139106\n", 159 | "\u001b[0m Terminal dJ: \u001b[32m7.166105297784497e-5\n", 160 | "\u001b[0m Terminal gradient: \u001b[32m0.0018706099261702532\n", 161 | "\u001b[0m Terminal constraint violation: \u001b[32m5.530829798594539e-7\n", 162 | "\u001b[0m Solve Status: \u001b[1m\u001b[32mSOLVE_SUCCEEDED\n", 163 | "\u001b[0m" 164 | ] 165 | } 166 | ], 167 | "source": [ 168 | "opts = SolverOptions(\n", 169 | " cost_tolerance_intermediate=1e-3,\n", 170 | " penalty_scaling=10.,\n", 171 | " penalty_initial=1.0\n", 172 | ")\n", 173 | "\n", 174 | "altro = ALTROSolver(prob, opts)\n", 175 | "set_options!(altro, show_summary=true)\n", 176 | "solve!(altro);" 177 | ] 178 | }, 179 | { 180 | "cell_type": "code", 181 | "execution_count": 29, 182 | "id": "428de99e-b15f-498e-96db-c8addee6f9b3", 183 | "metadata": {}, 184 | "outputs": [], 185 | "source": [ 186 | "# Extract the solution\n", 187 | "X = states(altro);\n", 188 | "U = controls(altro);" 189 | ] 190 | }, 191 | { 192 | "cell_type": "code", 193 | "execution_count": 30, 194 | "id": "d367bd6c-d031-4da0-8121-5da96901456b", 195 | "metadata": {}, 196 | "outputs": [ 197 | { 198 | "name": "stderr", 199 | "output_type": "stream", 200 | "text": [ 201 | "┌ Info: MeshCat server started. You can open the visualizer by visiting the following URL in your browser:\n", 202 | "│ http://127.0.0.1:8715\n", 203 | "└ @ MeshCat /Users/zac/.julia/packages/MeshCat/GlCMx/src/visualizer.jl:73\n" 204 | ] 205 | }, 206 | { 207 | "data": { 208 | "text/html": [ 209 | "
\n", 210 | " \n", 211 | "
\n" 212 | ], 213 | "text/plain": [ 214 | "MeshCat.DisplayedVisualizer(MeshCat.CoreVisualizer(MeshCat.SceneTrees.SceneNode(nothing, nothing, Dict{String, Vector{UInt8}}(), nothing, Dict{String, MeshCat.SceneTrees.SceneNode}()), Set{Any}(), ip\"127.0.0.1\", 8715))" 215 | ] 216 | }, 217 | "execution_count": 30, 218 | "metadata": {}, 219 | "output_type": "execute_result" 220 | } 221 | ], 222 | "source": [ 223 | "using TrajOptPlots\n", 224 | "using MeshCat\n", 225 | "using Plots\n", 226 | "\n", 227 | "vis = Visualizer()\n", 228 | "render(vis)" 229 | ] 230 | }, 231 | { 232 | "cell_type": "code", 233 | "execution_count": 31, 234 | "id": "5fcc35fa-990c-4dcb-8edc-7846da9fb966", 235 | "metadata": {}, 236 | "outputs": [ 237 | { 238 | "data": { 239 | "text/plain": [ 240 | "MeshCat Visualizer with path /meshcat/robot/cart/pole at http://127.0.0.1:8715" 241 | ] 242 | }, 243 | "execution_count": 31, 244 | "metadata": {}, 245 | "output_type": "execute_result" 246 | } 247 | ], 248 | "source": [ 249 | "TrajOptPlots.set_mesh!(vis, model)" 250 | ] 251 | }, 252 | { 253 | "cell_type": "code", 254 | "execution_count": 32, 255 | "id": "7b124f7f-26a3-4a74-ace1-0735fe716a84", 256 | "metadata": {}, 257 | "outputs": [], 258 | "source": [ 259 | "visualize!(vis, altro);" 260 | ] 261 | }, 262 | { 263 | "cell_type": "code", 264 | "execution_count": 15, 265 | "id": "79b1dacd-915a-41de-9bff-146ae73f2532", 266 | "metadata": {}, 267 | "outputs": [ 268 | { 269 | "data": { 270 | "image/svg+xml": [ 271 | "\n", 272 | "\n", 273 | "\n", 274 | " \n", 275 | " \n", 276 | " \n", 277 | "\n", 278 | "\n", 281 | "\n", 282 | " \n", 283 | " \n", 284 | " \n", 285 | "\n", 286 | "\n", 289 | "\n", 290 | " \n", 291 | " \n", 292 | " \n", 293 | "\n", 294 | "\n", 297 | "\n", 300 | "\n", 303 | "\n", 306 | "\n", 309 | "\n", 312 | "\n", 315 | "\n", 318 | "\n", 321 | "\n", 324 | "\n", 327 | "\n", 330 | "\n", 333 | "\n", 336 | "\n", 339 | "\n", 342 | "\n", 345 | "\n", 348 | "\n", 351 | "\n", 354 | "\n", 357 | "\n", 360 | "\n", 363 | "\n", 366 | "\n", 369 | "\n", 372 | "\n", 385 | "\n", 388 | "\n", 391 | "\n", 394 | "\n" 395 | ] 396 | }, 397 | "execution_count": 15, 398 | "metadata": {}, 399 | "output_type": "execute_result" 400 | } 401 | ], 402 | "source": [ 403 | "plot(U)" 404 | ] 405 | }, 406 | { 407 | "cell_type": "code", 408 | "execution_count": null, 409 | "id": "d5bfc784-e6b2-4505-86a0-dcf5381e5cd4", 410 | "metadata": {}, 411 | "outputs": [], 412 | "source": [] 413 | } 414 | ], 415 | "metadata": { 416 | "kernelspec": { 417 | "display_name": "Julia 1.6.5", 418 | "language": "julia", 419 | "name": "julia-1.6" 420 | }, 421 | "language_info": { 422 | "file_extension": ".jl", 423 | "mimetype": "application/julia", 424 | "name": "julia", 425 | "version": "1.6.5" 426 | } 427 | }, 428 | "nbformat": 4, 429 | "nbformat_minor": 5 430 | } 431 | -------------------------------------------------------------------------------- /Lecture 12/Lecture 12.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 12/Lecture 12.pdf -------------------------------------------------------------------------------- /Lecture 12/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 12/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 12/Project.toml -------------------------------------------------------------------------------- /Lecture 13/Lecture 13.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 13/Lecture 13.pdf -------------------------------------------------------------------------------- /Lecture 14/Lecture 14.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 14/Lecture 14.pdf -------------------------------------------------------------------------------- /Lecture 14/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 14/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 14/Project.toml -------------------------------------------------------------------------------- /Lecture 14/wahba.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using ForwardDiff" 20 | ] 21 | }, 22 | { 23 | "cell_type": "code", 24 | "execution_count": null, 25 | "metadata": {}, 26 | "outputs": [], 27 | "source": [ 28 | "function hat(v)\n", 29 | " return [0 -v[3] v[2];\n", 30 | " v[3] 0 -v[1];\n", 31 | " -v[2] v[1] 0]\n", 32 | "end" 33 | ] 34 | }, 35 | { 36 | "cell_type": "code", 37 | "execution_count": null, 38 | "metadata": {}, 39 | "outputs": [], 40 | "source": [ 41 | "function L(q)\n", 42 | " s = q[1]\n", 43 | " v = q[2:4]\n", 44 | " L = [s -v';\n", 45 | " v s*I+hat(v)]\n", 46 | " return L\n", 47 | "end" 48 | ] 49 | }, 50 | { 51 | "cell_type": "code", 52 | "execution_count": null, 53 | "metadata": {}, 54 | "outputs": [], 55 | "source": [ 56 | "function R(q)\n", 57 | " s = q[1]\n", 58 | " v = q[2:4]\n", 59 | " R = [s -v';\n", 60 | " v s*I-hat(v)]\n", 61 | " return R\n", 62 | "end" 63 | ] 64 | }, 65 | { 66 | "cell_type": "code", 67 | "execution_count": null, 68 | "metadata": {}, 69 | "outputs": [], 70 | "source": [ 71 | "T = Diagonal([1; -ones(3)])\n", 72 | "H = [zeros(1,3); I];" 73 | ] 74 | }, 75 | { 76 | "cell_type": "code", 77 | "execution_count": null, 78 | "metadata": {}, 79 | "outputs": [], 80 | "source": [ 81 | "function G(q)\n", 82 | " G = L(q)*H\n", 83 | "end\n", 84 | "\n", 85 | "function Q(q)\n", 86 | " return H'*(R(q)'*L(q))*H\n", 87 | "end" 88 | ] 89 | }, 90 | { 91 | "cell_type": "code", 92 | "execution_count": null, 93 | "metadata": {}, 94 | "outputs": [], 95 | "source": [ 96 | "#Generate a random quaternion\n", 97 | "qtrue = randn(4)\n", 98 | "qtrue = qtrue/norm(qtrue)\n", 99 | "\n", 100 | "Qtrue = Q(qtrue) #Generate equivalent rotation matrix" 101 | ] 102 | }, 103 | { 104 | "cell_type": "code", 105 | "execution_count": null, 106 | "metadata": {}, 107 | "outputs": [], 108 | "source": [ 109 | "#Generate data\n", 110 | "vN = randn(3,10) #Generate some random world-frame vectors\n", 111 | "\n", 112 | "#normalize\n", 113 | "for k = 1:10\n", 114 | " vN[:,k] .= vN[:,k]./norm(vN[:,k])\n", 115 | "end\n", 116 | "\n", 117 | "vB = Qtrue'*vN #generate body-frame vectors" 118 | ] 119 | }, 120 | { 121 | "cell_type": "code", 122 | "execution_count": null, 123 | "metadata": {}, 124 | "outputs": [], 125 | "source": [ 126 | "function residual(q)\n", 127 | " r = vN - Q(q)*vB\n", 128 | " return r[:]\n", 129 | "end" 130 | ] 131 | }, 132 | { 133 | "cell_type": "code", 134 | "execution_count": null, 135 | "metadata": {}, 136 | "outputs": [], 137 | "source": [ 138 | "#Random initial guess\n", 139 | "q = randn(4)\n", 140 | "q = q/norm(q)" 141 | ] 142 | }, 143 | { 144 | "cell_type": "code", 145 | "execution_count": null, 146 | "metadata": {}, 147 | "outputs": [], 148 | "source": [ 149 | "#Gauss-Newton Method\n", 150 | "ϕ = ones(3)\n", 151 | "iter = 0\n", 152 | "while maximum(abs.(ϕ)) > 1e-8\n", 153 | " r = residual(q)\n", 154 | " dr = ForwardDiff.jacobian(residual, q)\n", 155 | " ∇r = dr*G(q)\n", 156 | " ϕ = -(∇r'*∇r)\\(∇r'*r) #3-parameter update computed with gauss-newton\n", 157 | " q = L(q)*[sqrt(1-ϕ'*ϕ); ϕ] #multiplicative update applied to q\n", 158 | " iter += 1\n", 159 | "end" 160 | ] 161 | }, 162 | { 163 | "cell_type": "code", 164 | "execution_count": null, 165 | "metadata": {}, 166 | "outputs": [], 167 | "source": [ 168 | "iter" 169 | ] 170 | }, 171 | { 172 | "cell_type": "code", 173 | "execution_count": null, 174 | "metadata": {}, 175 | "outputs": [], 176 | "source": [ 177 | "q-qtrue" 178 | ] 179 | }, 180 | { 181 | "cell_type": "code", 182 | "execution_count": null, 183 | "metadata": {}, 184 | "outputs": [], 185 | "source": [ 186 | "q+qtrue" 187 | ] 188 | }, 189 | { 190 | "cell_type": "code", 191 | "execution_count": null, 192 | "metadata": {}, 193 | "outputs": [], 194 | "source": [] 195 | } 196 | ], 197 | "metadata": { 198 | "kernelspec": { 199 | "display_name": "Julia 1.6.5", 200 | "language": "julia", 201 | "name": "julia-1.6" 202 | }, 203 | "language_info": { 204 | "file_extension": ".jl", 205 | "mimetype": "application/julia", 206 | "name": "julia", 207 | "version": "1.6.5" 208 | } 209 | }, 210 | "nbformat": 4, 211 | "nbformat_minor": 4 212 | } 213 | -------------------------------------------------------------------------------- /Lecture 15/Lecture 15.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 15/Lecture 15.pdf -------------------------------------------------------------------------------- /Lecture 15/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 15/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 15/Project.toml -------------------------------------------------------------------------------- /Lecture 15/quadrotor.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using ForwardDiff\n", 20 | "using BlockDiagonals\n", 21 | "using ControlSystems" 22 | ] 23 | }, 24 | { 25 | "cell_type": "code", 26 | "execution_count": null, 27 | "metadata": {}, 28 | "outputs": [], 29 | "source": [ 30 | "#Quaternion stuff\n", 31 | "function hat(v)\n", 32 | " return [0 -v[3] v[2];\n", 33 | " v[3] 0 -v[1];\n", 34 | " -v[2] v[1] 0]\n", 35 | "end\n", 36 | "function L(q)\n", 37 | " s = q[1]\n", 38 | " v = q[2:4]\n", 39 | " L = [s -v';\n", 40 | " v s*I+hat(v)]\n", 41 | " return L\n", 42 | "end\n", 43 | "T = Diagonal([1; -ones(3)])\n", 44 | "H = [zeros(1,3); I]\n", 45 | "function qtoQ(q)\n", 46 | " return H'*T*L(q)*T*L(q)*H\n", 47 | "end\n", 48 | "function G(q)\n", 49 | " G = L(q)*H\n", 50 | "end\n", 51 | "function rptoq(ϕ)\n", 52 | " (1/sqrt(1+ϕ'*ϕ))*[1; ϕ]\n", 53 | "end\n", 54 | "function qtorp(q)\n", 55 | " q[2:4]/q[1]\n", 56 | "end" 57 | ] 58 | }, 59 | { 60 | "cell_type": "code", 61 | "execution_count": null, 62 | "metadata": {}, 63 | "outputs": [], 64 | "source": [ 65 | "#Quadrotor parameters\n", 66 | "m = 0.5\n", 67 | "ℓ = 0.1750\n", 68 | "J = Diagonal([0.0023, 0.0023, 0.004])\n", 69 | "g = 9.81\n", 70 | "kt=1.0\n", 71 | "km=0.0245\n", 72 | "\n", 73 | "h = 0.05 #20 Hz\n", 74 | "\n", 75 | "Nx = 13 # number of states (quaternion)\n", 76 | "Nx̃ = 12 # number of states (linearized)\n", 77 | "Nu = 4 # number of controls\n", 78 | "Tfinal = 5.0 # final time\n", 79 | "Nt = Int(Tfinal/h)+1 # number of time steps\n", 80 | "thist = Array(range(0,h*(Nt-1), step=h));" 81 | ] 82 | }, 83 | { 84 | "cell_type": "code", 85 | "execution_count": null, 86 | "metadata": {}, 87 | "outputs": [], 88 | "source": [ 89 | "function E(q)\n", 90 | " E = BlockDiagonal([1.0*I(3), G(q), 1.0*I(6)])\n", 91 | "end" 92 | ] 93 | }, 94 | { 95 | "cell_type": "code", 96 | "execution_count": null, 97 | "metadata": {}, 98 | "outputs": [], 99 | "source": [ 100 | "function quad_dynamics(x,u)\n", 101 | " r = x[1:3]\n", 102 | " q = x[4:7]\n", 103 | " v = x[8:10]\n", 104 | " ω = x[11:13]\n", 105 | " Q = qtoQ(q)\n", 106 | " \n", 107 | " ṙ = Q*v\n", 108 | " q̇ = 0.5*L(q)*H*ω\n", 109 | " \n", 110 | " v̇ = Q'*[0; 0; -g] + (1/m)*[zeros(2,4); kt*ones(1,4)]*u - hat(ω)*v\n", 111 | " \n", 112 | " ω̇ = J\\(-hat(ω)*J*ω + [0 ℓ*kt 0 -ℓ*kt; -ℓ*kt 0 ℓ*kt 0; km -km km -km]*u)\n", 113 | " \n", 114 | " return [ṙ; q̇; v̇; ω̇]\n", 115 | "end" 116 | ] 117 | }, 118 | { 119 | "cell_type": "code", 120 | "execution_count": null, 121 | "metadata": {}, 122 | "outputs": [], 123 | "source": [ 124 | "function quad_dynamics_rk4(x,u)\n", 125 | " #RK4 integration with zero-order hold on u\n", 126 | " f1 = quad_dynamics(x, u)\n", 127 | " f2 = quad_dynamics(x + 0.5*h*f1, u)\n", 128 | " f3 = quad_dynamics(x + 0.5*h*f2, u)\n", 129 | " f4 = quad_dynamics(x + h*f3, u)\n", 130 | " xn = x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 131 | " xn[4:7] .= xn[4:7]/norm(xn[4:7]) #re-normalize quaternion\n", 132 | " return xn\n", 133 | "end" 134 | ] 135 | }, 136 | { 137 | "cell_type": "code", 138 | "execution_count": null, 139 | "metadata": {}, 140 | "outputs": [], 141 | "source": [ 142 | "#Initial Conditions\n", 143 | "uhover = (m*g/4)*ones(4)\n", 144 | "r0 = [0.0; 0; 1.0]\n", 145 | "q0 = [1.0; 0; 0; 0]\n", 146 | "v0 = zeros(3)\n", 147 | "ω0 = zeros(3)\n", 148 | "x0 = [r0; q0; v0; ω0];" 149 | ] 150 | }, 151 | { 152 | "cell_type": "code", 153 | "execution_count": null, 154 | "metadata": {}, 155 | "outputs": [], 156 | "source": [ 157 | "#Linearize dynamics about hover\n", 158 | "A = ForwardDiff.jacobian(x->quad_dynamics_rk4(x,uhover),x0)\n", 159 | "B = ForwardDiff.jacobian(u->quad_dynamics_rk4(x0,u),uhover);" 160 | ] 161 | }, 162 | { 163 | "cell_type": "code", 164 | "execution_count": null, 165 | "metadata": {}, 166 | "outputs": [], 167 | "source": [ 168 | "rank(A)#The naive A matrix will always be rank deficient due to the unit-norm constraint on the quaternion" 169 | ] 170 | }, 171 | { 172 | "cell_type": "code", 173 | "execution_count": null, 174 | "metadata": {}, 175 | "outputs": [], 176 | "source": [ 177 | "#Controlability Matrix\n", 178 | "C = B\n", 179 | "for k = 1:(Nx-1)\n", 180 | " C = [C A*C[:,end-(Nu-1):end]]\n", 181 | "end" 182 | ] 183 | }, 184 | { 185 | "cell_type": "code", 186 | "execution_count": null, 187 | "metadata": {}, 188 | "outputs": [], 189 | "source": [ 190 | "rank(C) #The naive linearized system will always be uncontrollable" 191 | ] 192 | }, 193 | { 194 | "cell_type": "code", 195 | "execution_count": null, 196 | "metadata": {}, 197 | "outputs": [], 198 | "source": [ 199 | "#Reduced system\n", 200 | "Ã = Array(E(q0)'*A*E(q0))\n", 201 | "B̃ = Array(E(q0)'*B);" 202 | ] 203 | }, 204 | { 205 | "cell_type": "code", 206 | "execution_count": null, 207 | "metadata": {}, 208 | "outputs": [], 209 | "source": [ 210 | "#Controlability Matrix\n", 211 | "C = B̃\n", 212 | "for k = 1:(Nx-1)\n", 213 | " C = [C Ã*C[:,end-(Nu-1):end]]\n", 214 | "end" 215 | ] 216 | }, 217 | { 218 | "cell_type": "code", 219 | "execution_count": null, 220 | "metadata": {}, 221 | "outputs": [], 222 | "source": [ 223 | "rank(C) #Reduced system is controllable" 224 | ] 225 | }, 226 | { 227 | "cell_type": "code", 228 | "execution_count": null, 229 | "metadata": {}, 230 | "outputs": [], 231 | "source": [ 232 | "# Cost weights\n", 233 | "Q = Array(I(Nx̃));\n", 234 | "R = Array(.1*I(Nu));" 235 | ] 236 | }, 237 | { 238 | "cell_type": "code", 239 | "execution_count": null, 240 | "metadata": {}, 241 | "outputs": [], 242 | "source": [ 243 | "#LQR Controller\n", 244 | "K = dlqr(Ã,B̃,Q,R)" 245 | ] 246 | }, 247 | { 248 | "cell_type": "code", 249 | "execution_count": null, 250 | "metadata": {}, 251 | "outputs": [], 252 | "source": [ 253 | "#Feedback controller\n", 254 | "function controller(x)\n", 255 | " \n", 256 | " q0 = x0[4:7]\n", 257 | " q = x[4:7]\n", 258 | " ϕ = qtorp(L(q0)'*q)\n", 259 | " \n", 260 | " Δx̃ = [x[1:3]-r0; ϕ; x[8:10]-v0; x[11:13]-ω0]\n", 261 | " \n", 262 | " u = uhover - K*Δx̃\n", 263 | "end" 264 | ] 265 | }, 266 | { 267 | "cell_type": "code", 268 | "execution_count": null, 269 | "metadata": {}, 270 | "outputs": [], 271 | "source": [ 272 | "#Simulation\n", 273 | "uhist = zeros(Nu,Nt)\n", 274 | "xhist = zeros(Nx,Nt)\n", 275 | "xhist[:,1] = [r0+randn(3); L(q0)*rptoq(0.7*randn(3)); v0; ω0]\n", 276 | "for k = 1:(Nt-1)\n", 277 | " uhist[:,k] = controller(xhist[:,k])\n", 278 | " xhist[:,k+1] = quad_dynamics_rk4(xhist[:,k],uhist[:,k])\n", 279 | "end" 280 | ] 281 | }, 282 | { 283 | "cell_type": "code", 284 | "execution_count": null, 285 | "metadata": {}, 286 | "outputs": [], 287 | "source": [ 288 | "#Set up visualization\n", 289 | "using TrajOptPlots\n", 290 | "using MeshCat\n", 291 | "using StaticArrays\n", 292 | "using RobotZoo:Quadrotor\n", 293 | "\n", 294 | "vis = Visualizer()\n", 295 | "render(vis)" 296 | ] 297 | }, 298 | { 299 | "cell_type": "code", 300 | "execution_count": null, 301 | "metadata": {}, 302 | "outputs": [], 303 | "source": [ 304 | "model = Quadrotor()\n", 305 | "TrajOptPlots.set_mesh!(vis, model)" 306 | ] 307 | }, 308 | { 309 | "cell_type": "code", 310 | "execution_count": null, 311 | "metadata": {}, 312 | "outputs": [], 313 | "source": [ 314 | "X1 = [SVector{13}(x) for x in eachcol(xhist)];\n", 315 | "visualize!(vis, model, thist[end], X1)" 316 | ] 317 | }, 318 | { 319 | "cell_type": "code", 320 | "execution_count": null, 321 | "metadata": {}, 322 | "outputs": [], 323 | "source": [] 324 | } 325 | ], 326 | "metadata": { 327 | "kernelspec": { 328 | "display_name": "Julia 1.6.5", 329 | "language": "julia", 330 | "name": "julia-1.6" 331 | }, 332 | "language_info": { 333 | "file_extension": ".jl", 334 | "mimetype": "application/julia", 335 | "name": "julia", 336 | "version": "1.6.5" 337 | } 338 | }, 339 | "nbformat": 4, 340 | "nbformat_minor": 4 341 | } 342 | -------------------------------------------------------------------------------- /Lecture 16/Lecture 16.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 16/Lecture 16.pdf -------------------------------------------------------------------------------- /Lecture 16/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | [[Adapt]] 4 | deps = ["LinearAlgebra"] 5 | git-tree-sha1 = "af92965fb30777147966f58acb05da51c5616b5f" 6 | uuid = "79e6a3ab-5dfb-504d-930d-738a2a938a0e" 7 | version = "3.3.3" 8 | 9 | [[ArgTools]] 10 | uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f" 11 | 12 | [[Artifacts]] 13 | uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33" 14 | 15 | [[Base64]] 16 | uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" 17 | 18 | [[DataAPI]] 19 | git-tree-sha1 = "cc70b17275652eb47bc9e5f81635981f13cea5c8" 20 | uuid = "9a962f9c-6df0-11e9-0e5d-c546b8b5ee8a" 21 | version = "1.9.0" 22 | 23 | [[DataValueInterfaces]] 24 | git-tree-sha1 = "bfc1187b79289637fa0ef6d4436ebdfe6905cbd6" 25 | uuid = "e2d170a0-9d28-54be-80f0-106bbe20a464" 26 | version = "1.0.0" 27 | 28 | [[Dates]] 29 | deps = ["Printf"] 30 | uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" 31 | 32 | [[Downloads]] 33 | deps = ["ArgTools", "LibCURL", "NetworkOptions"] 34 | uuid = "f43a241f-c20a-4ad4-852c-f6b1247861c6" 35 | 36 | [[EarCut_jll]] 37 | deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"] 38 | git-tree-sha1 = "3f3a2501fa7236e9b911e0f7a588c657e822bb6d" 39 | uuid = "5ae413db-bbd1-5e63-b57d-d24a61df00f5" 40 | version = "2.2.3+0" 41 | 42 | [[GeometryBasics]] 43 | deps = ["EarCut_jll", "IterTools", "LinearAlgebra", "StaticArrays", "StructArrays", "Tables"] 44 | git-tree-sha1 = "83ea630384a13fc4f002b77690bc0afeb4255ac9" 45 | uuid = "5c1252a2-5f33-56bf-86c9-59e7332b4326" 46 | version = "0.4.2" 47 | 48 | [[InteractiveUtils]] 49 | deps = ["Markdown"] 50 | uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" 51 | 52 | [[IterTools]] 53 | git-tree-sha1 = "fa6287a4469f5e048d763df38279ee729fbd44e5" 54 | uuid = "c8e1da08-722c-5040-9ed9-7db0dc04731e" 55 | version = "1.4.0" 56 | 57 | [[IteratorInterfaceExtensions]] 58 | git-tree-sha1 = "a3f24677c21f5bbe9d2a714f95dcd58337fb2856" 59 | uuid = "82899510-4779-5014-852e-03e436cf321d" 60 | version = "1.0.0" 61 | 62 | [[JLLWrappers]] 63 | deps = ["Preferences"] 64 | git-tree-sha1 = "abc9885a7ca2052a736a600f7fa66209f96506e1" 65 | uuid = "692b3bcd-3c85-4b1f-b108-f13ce0eb3210" 66 | version = "1.4.1" 67 | 68 | [[LibCURL]] 69 | deps = ["LibCURL_jll", "MozillaCACerts_jll"] 70 | uuid = "b27032c2-a3e7-50c8-80cd-2d36dbcbfd21" 71 | 72 | [[LibCURL_jll]] 73 | deps = ["Artifacts", "LibSSH2_jll", "Libdl", "MbedTLS_jll", "Zlib_jll", "nghttp2_jll"] 74 | uuid = "deac9b47-8bc7-5906-a0fe-35ac56dc84c0" 75 | 76 | [[LibGit2]] 77 | deps = ["Base64", "NetworkOptions", "Printf", "SHA"] 78 | uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" 79 | 80 | [[LibSSH2_jll]] 81 | deps = ["Artifacts", "Libdl", "MbedTLS_jll"] 82 | uuid = "29816b5a-b9ab-546f-933c-edad1886dfa8" 83 | 84 | [[Libdl]] 85 | uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" 86 | 87 | [[LinearAlgebra]] 88 | deps = ["Libdl"] 89 | uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 90 | 91 | [[Logging]] 92 | uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" 93 | 94 | [[Markdown]] 95 | deps = ["Base64"] 96 | uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" 97 | 98 | [[MbedTLS_jll]] 99 | deps = ["Artifacts", "Libdl"] 100 | uuid = "c8ffd9c3-330d-5841-b78e-0817d7145fa1" 101 | 102 | [[MozillaCACerts_jll]] 103 | uuid = "14a3606d-f60d-562e-9121-12d972cd8159" 104 | 105 | [[NetworkOptions]] 106 | uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908" 107 | 108 | [[OrderedCollections]] 109 | git-tree-sha1 = "85f8e6578bf1f9ee0d11e7bb1b1456435479d47c" 110 | uuid = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" 111 | version = "1.4.1" 112 | 113 | [[Pkg]] 114 | deps = ["Artifacts", "Dates", "Downloads", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "Serialization", "TOML", "Tar", "UUIDs", "p7zip_jll"] 115 | uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" 116 | 117 | [[Preferences]] 118 | deps = ["TOML"] 119 | git-tree-sha1 = "d3538e7f8a790dc8903519090857ef8e1283eecd" 120 | uuid = "21216c6a-2e73-6563-6e65-726566657250" 121 | version = "1.2.5" 122 | 123 | [[Printf]] 124 | deps = ["Unicode"] 125 | uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" 126 | 127 | [[REPL]] 128 | deps = ["InteractiveUtils", "Markdown", "Sockets", "Unicode"] 129 | uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" 130 | 131 | [[Random]] 132 | deps = ["Serialization"] 133 | uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" 134 | 135 | [[SHA]] 136 | uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" 137 | 138 | [[Serialization]] 139 | uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" 140 | 141 | [[Sockets]] 142 | uuid = "6462fe0b-24de-5631-8697-dd941f90decc" 143 | 144 | [[SparseArrays]] 145 | deps = ["LinearAlgebra", "Random"] 146 | uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" 147 | 148 | [[StaticArrays]] 149 | deps = ["LinearAlgebra", "Random", "Statistics"] 150 | git-tree-sha1 = "6976fab022fea2ffea3d945159317556e5dad87c" 151 | uuid = "90137ffa-7385-5640-81b9-e52037218182" 152 | version = "1.4.2" 153 | 154 | [[Statistics]] 155 | deps = ["LinearAlgebra", "SparseArrays"] 156 | uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" 157 | 158 | [[StructArrays]] 159 | deps = ["Adapt", "DataAPI", "StaticArrays", "Tables"] 160 | git-tree-sha1 = "57617b34fa34f91d536eb265df67c2d4519b8b98" 161 | uuid = "09ab397b-f2b6-538f-b94a-2f83cf4a842a" 162 | version = "0.6.5" 163 | 164 | [[TOML]] 165 | deps = ["Dates"] 166 | uuid = "fa267f1f-6049-4f14-aa54-33bafae1ed76" 167 | 168 | [[TableTraits]] 169 | deps = ["IteratorInterfaceExtensions"] 170 | git-tree-sha1 = "c06b2f539df1c6efa794486abfb6ed2022561a39" 171 | uuid = "3783bdb8-4a98-5b6b-af9a-565f29a5fe9c" 172 | version = "1.0.1" 173 | 174 | [[Tables]] 175 | deps = ["DataAPI", "DataValueInterfaces", "IteratorInterfaceExtensions", "LinearAlgebra", "OrderedCollections", "TableTraits", "Test"] 176 | git-tree-sha1 = "5ce79ce186cc678bbb5c5681ca3379d1ddae11a1" 177 | uuid = "bd369af6-aec1-5ad0-b16a-f7cc5008161c" 178 | version = "1.7.0" 179 | 180 | [[Tar]] 181 | deps = ["ArgTools", "SHA"] 182 | uuid = "a4e569a6-e804-4fa4-b0f3-eef7a1d5b13e" 183 | 184 | [[Test]] 185 | deps = ["InteractiveUtils", "Logging", "Random", "Serialization"] 186 | uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40" 187 | 188 | [[UUIDs]] 189 | deps = ["Random", "SHA"] 190 | uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" 191 | 192 | [[Unicode]] 193 | uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" 194 | 195 | [[Zlib_jll]] 196 | deps = ["Libdl"] 197 | uuid = "83775a58-1f1d-513f-b197-d71354ab007a" 198 | 199 | [[nghttp2_jll]] 200 | deps = ["Artifacts", "Libdl"] 201 | uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d" 202 | 203 | [[p7zip_jll]] 204 | deps = ["Artifacts", "Libdl"] 205 | uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0" 206 | -------------------------------------------------------------------------------- /Lecture 16/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | GeometryBasics = "5c1252a2-5f33-56bf-86c9-59e7332b4326" 3 | -------------------------------------------------------------------------------- /Lecture 16/hopper.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using PyPlot\n", 20 | "using ForwardDiff\n", 21 | "using Ipopt\n", 22 | "using MathOptInterface\n", 23 | "const MOI = MathOptInterface;" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": null, 29 | "metadata": {}, 30 | "outputs": [], 31 | "source": [ 32 | "Nx = 8 # number of state\n", 33 | "Nu = 2 # number of controls\n", 34 | "Tfinal = 4.4 # final time\n", 35 | "h = 0.1 #10 hz\n", 36 | "Nm = 5 #number of steps in each mode\n", 37 | "Nt = Int(ceil(Tfinal/h)+1) # number of time steps\n", 38 | "Nmodes = Int(ceil(Nt/Nm))\n", 39 | "thist = Array(range(0,h*(Nt-1), step=h));\n", 40 | "n_nlp = Nx*Nt + Nu*(Nt-1) # number of decision variables\n", 41 | "c_init_inds = 1:Nx\n", 42 | "c_term_inds = (c_init_inds[end]+1):(c_init_inds[end]+Nx)\n", 43 | "c_dyn_inds = (c_term_inds[end]+1):(c_term_inds[end]+Nx*(Nt-1))\n", 44 | "c_stance_inds = (c_dyn_inds[end]+1):(c_dyn_inds[end]+Int(ceil(Nmodes/2)*Nm))\n", 45 | "c_length_inds = (c_stance_inds[end]+1):(c_stance_inds[end]+Nt)\n", 46 | "m_nlp = c_length_inds[end]" 47 | ] 48 | }, 49 | { 50 | "cell_type": "code", 51 | "execution_count": null, 52 | "metadata": {}, 53 | "outputs": [], 54 | "source": [ 55 | "#Hopper Dynamics\n", 56 | "g = 9.81\n", 57 | "m1 = 5.0 #body mass\n", 58 | "m2 = 1.0 #foot mass\n", 59 | "ℓ_min = 0.5 #minimum length\n", 60 | "ℓ_max = 1.5 #maximum length\n", 61 | "\n", 62 | "function flight_dynamics(x,u)\n", 63 | " M = Diagonal([m1 m1 m2 m2])\n", 64 | " \n", 65 | " r1 = x[1:2]\n", 66 | " r2 = x[3:4]\n", 67 | " v = x[5:8]\n", 68 | " \n", 69 | " ℓ1 = (r1[1]-r2[1])/norm(r1-r2)\n", 70 | " ℓ2 = (r1[2]-r2[2])/norm(r1-r2)\n", 71 | " B = [ℓ1 ℓ2;\n", 72 | " ℓ2 -ℓ1;\n", 73 | " -ℓ1 -ℓ2;\n", 74 | " -ℓ2 ℓ1]\n", 75 | " \n", 76 | " v̇ = [0; -g; 0; -g] + M\\(B*u)\n", 77 | " \n", 78 | " ẋ = [v; v̇]\n", 79 | "end\n", 80 | "\n", 81 | "function stance_dynamics(x,u)\n", 82 | " m1 = 5.0 #body mass\n", 83 | " m2 = 1.0 #foot mass\n", 84 | " M = Diagonal([m1 m1 m2 m2])\n", 85 | " g = 9.81\n", 86 | " \n", 87 | " r1 = x[1:2]\n", 88 | " r2 = x[3:4]\n", 89 | " v = x[5:8]\n", 90 | " \n", 91 | " ℓ1 = (r1[1]-r2[1])/norm(r1-r2)\n", 92 | " ℓ2 = (r1[2]-r2[2])/norm(r1-r2)\n", 93 | " B = [ℓ1 ℓ2;\n", 94 | " ℓ2 -ℓ1;\n", 95 | " 0 0;\n", 96 | " 0 0]\n", 97 | " \n", 98 | " v̇ = [0; -g; 0; 0] + M\\(B*u)\n", 99 | " \n", 100 | " ẋ = [v; v̇]\n", 101 | "end\n", 102 | "\n", 103 | "function flight_dynamics_rk4(x,u)\n", 104 | " #RK4 integration with zero-order hold on u\n", 105 | " f1 = flight_dynamics(x, u)\n", 106 | " f2 = flight_dynamics(x + 0.5*h*f1, u)\n", 107 | " f3 = flight_dynamics(x + 0.5*h*f2, u)\n", 108 | " f4 = flight_dynamics(x + h*f3, u)\n", 109 | " return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 110 | "end\n", 111 | "\n", 112 | "function stance_dynamics_rk4(x,u)\n", 113 | " #RK4 integration with zero-order hold on u\n", 114 | " f1 = stance_dynamics(x, u)\n", 115 | " f2 = stance_dynamics(x + 0.5*h*f1, u)\n", 116 | " f3 = stance_dynamics(x + 0.5*h*f2, u)\n", 117 | " f4 = stance_dynamics(x + h*f3, u)\n", 118 | " return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 119 | "end\n", 120 | "\n", 121 | "function jump_map(x)\n", 122 | " #Assume the foot experiences inelastic collisions\n", 123 | " xn = [x[1:6]; 0.0; 0.0]\n", 124 | " return xn\n", 125 | "end" 126 | ] 127 | }, 128 | { 129 | "cell_type": "code", 130 | "execution_count": null, 131 | "metadata": {}, 132 | "outputs": [], 133 | "source": [ 134 | "# Cost weights\n", 135 | "Q = Diagonal([1.0*ones(4); 1.0*ones(4)]);\n", 136 | "R = 0.001;\n", 137 | "Qn = Q" 138 | ] 139 | }, 140 | { 141 | "cell_type": "code", 142 | "execution_count": null, 143 | "metadata": {}, 144 | "outputs": [], 145 | "source": [ 146 | "function stage_cost(x,u,k)\n", 147 | " return 0.5*((x-xref[:,k])'*Q*(x-xref[:,k])) + 0.5*(u-uref)'*R*(u-uref)\n", 148 | "end\n", 149 | "function terminal_cost(x)\n", 150 | " return 0.5*((x-xref[:,end])'*Qn*(x-xref[:,end]))\n", 151 | "end" 152 | ] 153 | }, 154 | { 155 | "cell_type": "code", 156 | "execution_count": null, 157 | "metadata": {}, 158 | "outputs": [], 159 | "source": [ 160 | "function cost(ztraj)\n", 161 | " z = reshape(ztraj[1:(end-Nx)],Nx+Nu,Nt-1)\n", 162 | " xtraj = [z[1:Nx,:] ztraj[end-(Nx-1):end]]\n", 163 | " utraj = z[(Nx+1):(Nx+Nu),:]\n", 164 | " J = 0.0\n", 165 | " for k = 1:(Nt-1)\n", 166 | " J += stage_cost(xtraj[:,k],utraj[:,k],k)\n", 167 | " end\n", 168 | " J += terminal_cost(xtraj[:,end])\n", 169 | " return J\n", 170 | "end" 171 | ] 172 | }, 173 | { 174 | "cell_type": "code", 175 | "execution_count": null, 176 | "metadata": {}, 177 | "outputs": [], 178 | "source": [ 179 | "#Reference Trajectory\n", 180 | "uref = [m1*g; 0.0]\n", 181 | "xref = zeros(Nx,Nt)\n", 182 | "xref[1,:] .= LinRange(-1.0,1.0,Nt)\n", 183 | "xref[2,:] .= 1.0 .+ 0.5.*sin.(2*pi/10.0*(0:(Nt-1)));\n", 184 | "xref[3,:] .= LinRange(-1.0,1.0,Nt)\n", 185 | "xref[5,2:end-1] .= (2.0/Tfinal)*ones(Nt-2)\n", 186 | "xref[7,2:end-1] .= (2.0/Tfinal)*ones(Nt-2);" 187 | ] 188 | }, 189 | { 190 | "cell_type": "code", 191 | "execution_count": null, 192 | "metadata": {}, 193 | "outputs": [], 194 | "source": [ 195 | "plot(xref[2,:])" 196 | ] 197 | }, 198 | { 199 | "cell_type": "code", 200 | "execution_count": null, 201 | "metadata": {}, 202 | "outputs": [], 203 | "source": [ 204 | "function dynamics_constraint!(c,ztraj)\n", 205 | " d = reshape(view(c,c_dyn_inds),Nx,Nt-1)\n", 206 | " z = reshape(ztraj[1:(end-Nx)],Nx+Nu,Nt-1)\n", 207 | " xtraj = [z[1:Nx,:] ztraj[end-(Nx-1):end]]\n", 208 | " utraj = z[(Nx+1):(Nx+Nu),:]\n", 209 | " for k = 1:(Nmodes-1)\n", 210 | " if mod(k,2) == 1\n", 211 | " for j = 1:Nm\n", 212 | " s = (k-1)*Nm + j\n", 213 | " d[:,s] = stance_dynamics_rk4(xtraj[:,s],utraj[:,s]) - xtraj[:,s+1]\n", 214 | " end\n", 215 | " else\n", 216 | " for j = 1:(Nm-1)\n", 217 | " s = (k-1)*Nm + j\n", 218 | " d[:,s] = flight_dynamics_rk4(xtraj[:,s],utraj[:,s]) - xtraj[:,s+1]\n", 219 | " end\n", 220 | " s = k*Nm\n", 221 | " d[:,s] = jump_map(flight_dynamics_rk4(xtraj[:,s],utraj[:,s])) - xtraj[:,s+1]\n", 222 | " end\n", 223 | " end\n", 224 | " if mod(Nmodes,2) == 1\n", 225 | " for j = 1:(Nm-1)\n", 226 | " s = (Nmodes-1)*Nm + j\n", 227 | " d[:,s] = stance_dynamics_rk4(xtraj[:,s],utraj[:,s]) - xtraj[:,s+1]\n", 228 | " end\n", 229 | " else\n", 230 | " for j = 1:(Nm-1)\n", 231 | " s = (Nmodes-1)*Nm + j\n", 232 | " d[:,s] = flight_dynamics_rk4(xtraj[:,s],utraj[:,s]) - xtraj[:,s+1]\n", 233 | " end\n", 234 | " end\n", 235 | " \n", 236 | " return nothing\n", 237 | "end" 238 | ] 239 | }, 240 | { 241 | "cell_type": "code", 242 | "execution_count": null, 243 | "metadata": {}, 244 | "outputs": [], 245 | "source": [ 246 | "function stance_constraint!(c,ztraj)\n", 247 | " d = view(c,c_stance_inds)\n", 248 | " z = reshape(ztraj[1:(end-Nx)],Nx+Nu,Nt-1)\n", 249 | " xtraj = [z[1:Nx,:] ztraj[(end-(Nx-1)):end]]\n", 250 | " t = 1\n", 251 | " for k = 1:Nmodes\n", 252 | " if mod(k,2) == 1\n", 253 | " for j = 1:Nm\n", 254 | " s = (k-1)*Nm + j\n", 255 | " d[t] = xtraj[4,s]\n", 256 | " t += 1\n", 257 | " end\n", 258 | " end\n", 259 | " \n", 260 | " end\n", 261 | " return nothing\n", 262 | "end" 263 | ] 264 | }, 265 | { 266 | "cell_type": "code", 267 | "execution_count": null, 268 | "metadata": {}, 269 | "outputs": [], 270 | "source": [ 271 | "function length_constraint!(c,ztraj)\n", 272 | " d = view(c,c_length_inds)\n", 273 | " z = reshape(ztraj[1:(end-Nx)],Nx+Nu,Nt-1)\n", 274 | " xtraj = [z[1:Nx,:] ztraj[(end-(Nx-1)):end]]\n", 275 | " for k = 1:Nt\n", 276 | " d[k] = norm(xtraj[1:2,k] - xtraj[3:4,k])\n", 277 | " end\n", 278 | "end" 279 | ] 280 | }, 281 | { 282 | "cell_type": "code", 283 | "execution_count": null, 284 | "metadata": {}, 285 | "outputs": [], 286 | "source": [ 287 | "function con!(c,ztraj)\n", 288 | " c[c_init_inds] .= ztraj[1:Nx] - xref[:,1] #initial state constraint\n", 289 | " c[c_term_inds] .= ztraj[(end-(Nx-1)):end] - xref[:,end] #terminal state constraint\n", 290 | " @views dynamics_constraint!(c,ztraj)\n", 291 | " @views stance_constraint!(c,ztraj)\n", 292 | " @views length_constraint!(c,ztraj)\n", 293 | "end" 294 | ] 295 | }, 296 | { 297 | "cell_type": "code", 298 | "execution_count": null, 299 | "metadata": {}, 300 | "outputs": [], 301 | "source": [ 302 | "struct ProblemMOI <: MOI.AbstractNLPEvaluator\n", 303 | " n_nlp::Int\n", 304 | " m_nlp::Int\n", 305 | " idx_ineq\n", 306 | " obj_grad::Bool\n", 307 | " con_jac::Bool\n", 308 | " sparsity_jac\n", 309 | " sparsity_hess\n", 310 | " primal_bounds\n", 311 | " constraint_bounds\n", 312 | " hessian_lagrangian::Bool\n", 313 | "end\n", 314 | "\n", 315 | "function ProblemMOI(n_nlp,m_nlp;\n", 316 | " idx_ineq=c_length_inds,\n", 317 | " obj_grad=true,\n", 318 | " con_jac=true,\n", 319 | " sparsity_jac=sparsity_jacobian(n_nlp,m_nlp),\n", 320 | " sparsity_hess=sparsity_hessian(n_nlp,m_nlp),\n", 321 | " primal_bounds=primal_bounds(n_nlp),\n", 322 | " constraint_bounds=constraint_bounds(m_nlp,idx_ineq=idx_ineq),\n", 323 | " hessian_lagrangian=false)\n", 324 | "\n", 325 | " ProblemMOI(n_nlp,m_nlp,\n", 326 | " idx_ineq,\n", 327 | " obj_grad,\n", 328 | " con_jac,\n", 329 | " sparsity_jac,\n", 330 | " sparsity_hess,\n", 331 | " primal_bounds,\n", 332 | " constraint_bounds,\n", 333 | " hessian_lagrangian)\n", 334 | "end\n", 335 | "\n", 336 | "function primal_bounds(n)\n", 337 | " x_l = -Inf*ones(n)\n", 338 | " x_u = Inf*ones(n)\n", 339 | " return x_l, x_u\n", 340 | "end\n", 341 | "\n", 342 | "function constraint_bounds(m; idx_ineq=(1:0))\n", 343 | " c_l = zeros(m)\n", 344 | " c_l[idx_ineq] .= ℓ_min\n", 345 | "\n", 346 | " c_u = zeros(m)\n", 347 | " c_u[idx_ineq] .= ℓ_max\n", 348 | " return c_l, c_u\n", 349 | "end\n", 350 | "\n", 351 | "function row_col!(row,col,r,c)\n", 352 | " for cc in c\n", 353 | " for rr in r\n", 354 | " push!(row,convert(Int,rr))\n", 355 | " push!(col,convert(Int,cc))\n", 356 | " end\n", 357 | " end\n", 358 | " return row, col\n", 359 | "end\n", 360 | "\n", 361 | "function sparsity_jacobian(n,m)\n", 362 | "\n", 363 | " row = []\n", 364 | " col = []\n", 365 | "\n", 366 | " r = 1:m\n", 367 | " c = 1:n\n", 368 | "\n", 369 | " row_col!(row,col,r,c)\n", 370 | "\n", 371 | " return collect(zip(row,col))\n", 372 | "end\n", 373 | "\n", 374 | "function sparsity_hessian(n,m)\n", 375 | "\n", 376 | " row = []\n", 377 | " col = []\n", 378 | "\n", 379 | " r = 1:m\n", 380 | " c = 1:n\n", 381 | "\n", 382 | " row_col!(row,col,r,c)\n", 383 | "\n", 384 | " return collect(zip(row,col))\n", 385 | "end\n", 386 | "\n", 387 | "function MOI.eval_objective(prob::MOI.AbstractNLPEvaluator, x)\n", 388 | " cost(x)\n", 389 | "end\n", 390 | "\n", 391 | "function MOI.eval_objective_gradient(prob::MOI.AbstractNLPEvaluator, grad_f, x)\n", 392 | " ForwardDiff.gradient!(grad_f,cost,x)\n", 393 | " return nothing\n", 394 | "end\n", 395 | "\n", 396 | "function MOI.eval_constraint(prob::MOI.AbstractNLPEvaluator,g,x)\n", 397 | " con!(g,x)\n", 398 | " return nothing\n", 399 | "end\n", 400 | "\n", 401 | "function MOI.eval_constraint_jacobian(prob::MOI.AbstractNLPEvaluator, jac, x)\n", 402 | " ForwardDiff.jacobian!(reshape(jac,prob.m_nlp,prob.n_nlp), con!, zeros(prob.m_nlp), x)\n", 403 | " return nothing\n", 404 | "end\n", 405 | "\n", 406 | "function MOI.features_available(prob::MOI.AbstractNLPEvaluator)\n", 407 | " return [:Grad, :Jac]\n", 408 | "end\n", 409 | "\n", 410 | "MOI.initialize(prob::MOI.AbstractNLPEvaluator, features) = nothing\n", 411 | "MOI.jacobian_structure(prob::MOI.AbstractNLPEvaluator) = prob.sparsity_jac\n", 412 | "\n", 413 | "function solve(x0,prob::MOI.AbstractNLPEvaluator;\n", 414 | " tol=1.0e-6,c_tol=1.0e-6,max_iter=10000)\n", 415 | " x_l, x_u = prob.primal_bounds\n", 416 | " c_l, c_u = prob.constraint_bounds\n", 417 | "\n", 418 | " nlp_bounds = MOI.NLPBoundsPair.(c_l,c_u)\n", 419 | " block_data = MOI.NLPBlockData(nlp_bounds,prob,true)\n", 420 | "\n", 421 | " solver = Ipopt.Optimizer()\n", 422 | " solver.options[\"max_iter\"] = max_iter\n", 423 | " solver.options[\"tol\"] = tol\n", 424 | " solver.options[\"constr_viol_tol\"] = c_tol\n", 425 | "\n", 426 | " x = MOI.add_variables(solver,prob.n_nlp)\n", 427 | "\n", 428 | " for i = 1:prob.n_nlp\n", 429 | " xi = MOI.SingleVariable(x[i])\n", 430 | " MOI.add_constraint(solver, xi, MOI.LessThan(x_u[i]))\n", 431 | " MOI.add_constraint(solver, xi, MOI.GreaterThan(x_l[i]))\n", 432 | " MOI.set(solver, MOI.VariablePrimalStart(), x[i], x0[i])\n", 433 | " end\n", 434 | "\n", 435 | " # Solve the problem\n", 436 | " MOI.set(solver, MOI.NLPBlock(), block_data)\n", 437 | " MOI.set(solver, MOI.ObjectiveSense(), MOI.MIN_SENSE)\n", 438 | " MOI.optimize!(solver)\n", 439 | "\n", 440 | " # Get the solution\n", 441 | " res = MOI.get(solver, MOI.VariablePrimal(), x)\n", 442 | "\n", 443 | " return res\n", 444 | "end" 445 | ] 446 | }, 447 | { 448 | "cell_type": "code", 449 | "execution_count": null, 450 | "metadata": {}, 451 | "outputs": [], 452 | "source": [ 453 | "#Guess\n", 454 | "xguess = xref + 0.1*randn(Nx,Nt)\n", 455 | "uguess = kron(ones(Nt-1)', uref) + 0.1*randn(Nu,Nt-1)\n", 456 | "z0 = [reshape([xguess[:,1:(Nt-1)]; uguess],(Nx+Nu)*(Nt-1),1); xguess[:,end]];" 457 | ] 458 | }, 459 | { 460 | "cell_type": "code", 461 | "execution_count": null, 462 | "metadata": {}, 463 | "outputs": [], 464 | "source": [ 465 | "prob = ProblemMOI(n_nlp,m_nlp)\n", 466 | "z_sol = solve(z0,prob) # solve" 467 | ] 468 | }, 469 | { 470 | "cell_type": "code", 471 | "execution_count": null, 472 | "metadata": {}, 473 | "outputs": [], 474 | "source": [ 475 | "z = reshape(z_sol[1:(end-Nx)],Nx+Nu,Nt-1)\n", 476 | "xtraj = [z[1:Nx,:] z_sol[end-(Nx-1):end]]\n", 477 | "utraj = z[(Nx+1):(Nx+Nu),:];" 478 | ] 479 | }, 480 | { 481 | "cell_type": "code", 482 | "execution_count": null, 483 | "metadata": {}, 484 | "outputs": [], 485 | "source": [ 486 | "plot(thist,xtraj[2,:])\n", 487 | "plot(thist,xtraj[4,:])" 488 | ] 489 | }, 490 | { 491 | "cell_type": "code", 492 | "execution_count": null, 493 | "metadata": {}, 494 | "outputs": [], 495 | "source": [ 496 | "plot(thist,xtraj[1,:])\n", 497 | "plot(thist,xtraj[3,:])" 498 | ] 499 | }, 500 | { 501 | "cell_type": "code", 502 | "execution_count": null, 503 | "metadata": {}, 504 | "outputs": [], 505 | "source": [ 506 | "plot(thist[1:end-1],utraj[1,:])" 507 | ] 508 | }, 509 | { 510 | "cell_type": "code", 511 | "execution_count": null, 512 | "metadata": {}, 513 | "outputs": [], 514 | "source": [ 515 | "plot(thist[1:end-1],utraj[2,:])" 516 | ] 517 | }, 518 | { 519 | "cell_type": "code", 520 | "execution_count": null, 521 | "metadata": {}, 522 | "outputs": [], 523 | "source": [ 524 | "#Set up visualizer\n", 525 | "using Colors\n", 526 | "using CoordinateTransformations\n", 527 | "using GeometryBasics\n", 528 | "using MeshCat\n", 529 | "using Rotations\n", 530 | "using StaticArrays\n", 531 | "\n", 532 | "# Kinematics\n", 533 | "kinematics(q) = [q[3], q[4]]\n", 534 | "\n", 535 | "# Visualization\n", 536 | "function visualize!(vis, q, Δt)\n", 537 | "\n", 538 | " # body radius\n", 539 | " r_body = 0.2\n", 540 | "\t# foot radius\n", 541 | " r_foot = 0.1\n", 542 | "\n", 543 | "\t# leg width\n", 544 | " r_leg = 0.5 * r_foot\n", 545 | "\n", 546 | "\t# set background to white\n", 547 | "\tsetvisible!(vis[\"/Background\"], true)\n", 548 | " setprop!(vis[\"/Background\"], \"top_color\", RGBA(1.0, 1.0, 1.0, 1.0))\n", 549 | " setprop!(vis[\"/Background\"], \"bottom_color\", RGBA(1.0, 1.0, 1.0, 1.0))\n", 550 | " setvisible!(vis[\"/Axes\"], false)\n", 551 | "\n", 552 | "\t# create body\n", 553 | " setobject!(vis[\"body\"], Sphere(Point3f0(0),\n", 554 | " convert(Float32, r_body)),\n", 555 | " MeshPhongMaterial(color = RGBA(0, 1, 0, 1.0)))\n", 556 | "\n", 557 | "\t# create foot\n", 558 | " setobject!(vis[\"foot\"], Sphere(Point3f0(0),\n", 559 | " convert(Float32, r_foot)),\n", 560 | " MeshPhongMaterial(color = RGBA(1.0, 165.0 / 255.0, 0, 1.0)))\n", 561 | "\n", 562 | "# \t# create leg\n", 563 | " n_leg = 100\n", 564 | " for i = 1:n_leg\n", 565 | " setobject!(vis[\"leg$i\"], Sphere(Point3f0(0),\n", 566 | " convert(Float32, r_leg)),\n", 567 | " MeshPhongMaterial(color = RGBA(0, 0, 0, 1.0)))\n", 568 | " end\n", 569 | "\n", 570 | " p_leg = [zeros(3) for i = 1:n_leg]\n", 571 | "\n", 572 | "\t# animation\n", 573 | " anim = MeshCat.Animation(convert(Int, floor(1.0 / Δt)))\n", 574 | "\n", 575 | " for t = 1:length(q)\n", 576 | " p_body = [q[t][1], 0.0, q[t][2]]\n", 577 | " p_foot = [kinematics(q[t])[1], 0.0, kinematics(q[t])[2]]\n", 578 | "\n", 579 | " dir = [q[t][3] - q[t][1]; q[t][4] - q[t][2]]\n", 580 | " dir = dir ./ norm(dir)\n", 581 | " r_range = range(0, stop = sqrt((q[t][1] - q[t][3])^2.0 + (q[t][2] - q[t][4])^2.0), length = n_leg)\n", 582 | " for i = 1:n_leg\n", 583 | " p_leg[i] = [q[t][1] + r_range[i] * dir[1], 0.0, q[t][2] + r_range[i] * dir[2]]\n", 584 | " end\n", 585 | " \n", 586 | " z_shift = [0.0; 0.0; r_foot]\n", 587 | "\n", 588 | " MeshCat.atframe(anim, t) do\n", 589 | " settransform!(vis[\"body\"], Translation(p_body + z_shift))\n", 590 | " settransform!(vis[\"foot\"], Translation(p_foot + z_shift))\n", 591 | "\n", 592 | " for i = 1:n_leg\n", 593 | " settransform!(vis[\"leg$i\"], Translation(p_leg[i] + z_shift))\n", 594 | " end\n", 595 | " end\n", 596 | " end\n", 597 | "\n", 598 | " MeshCat.setanimation!(vis, anim)\n", 599 | "end\n", 600 | "\n", 601 | "vis = Visualizer()\n", 602 | "render(vis)" 603 | ] 604 | }, 605 | { 606 | "cell_type": "code", 607 | "execution_count": null, 608 | "metadata": {}, 609 | "outputs": [], 610 | "source": [ 611 | "q = [SVector{4}(x[1:4]) for x in eachcol(xtraj)];\n", 612 | "visualize!(vis, q, h)" 613 | ] 614 | }, 615 | { 616 | "cell_type": "code", 617 | "execution_count": null, 618 | "metadata": {}, 619 | "outputs": [], 620 | "source": [] 621 | } 622 | ], 623 | "metadata": { 624 | "kernelspec": { 625 | "display_name": "Julia 1.6.5", 626 | "language": "julia", 627 | "name": "julia-1.6" 628 | }, 629 | "language_info": { 630 | "file_extension": ".jl", 631 | "mimetype": "application/julia", 632 | "name": "julia", 633 | "version": "1.6.5" 634 | } 635 | }, 636 | "nbformat": 4, 637 | "nbformat_minor": 4 638 | } 639 | -------------------------------------------------------------------------------- /Lecture 16/hybrid-ball.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using PyPlot" 20 | ] 21 | }, 22 | { 23 | "cell_type": "code", 24 | "execution_count": null, 25 | "metadata": {}, 26 | "outputs": [], 27 | "source": [ 28 | "#Continuous Dynamics\n", 29 | "function dynamics(x)\n", 30 | " g = 9.81\n", 31 | " \n", 32 | " r = x[1:2]\n", 33 | " v = x[3:4]\n", 34 | " \n", 35 | " v̇ = [0; -g]\n", 36 | " \n", 37 | " xdot = [v; v̇]\n", 38 | "end" 39 | ] 40 | }, 41 | { 42 | "cell_type": "code", 43 | "execution_count": null, 44 | "metadata": {}, 45 | "outputs": [], 46 | "source": [ 47 | "function dynamics_rk4(x)\n", 48 | " #RK4 integration with zero-order hold on u\n", 49 | " f1 = dynamics(x)\n", 50 | " f2 = dynamics(x + 0.5*h*f1)\n", 51 | " f3 = dynamics(x + 0.5*h*f2)\n", 52 | " f4 = dynamics(x + h*f3)\n", 53 | " return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 54 | "end" 55 | ] 56 | }, 57 | { 58 | "cell_type": "code", 59 | "execution_count": null, 60 | "metadata": {}, 61 | "outputs": [], 62 | "source": [ 63 | "#guard function\n", 64 | "function guard(x)\n", 65 | " return x[2]\n", 66 | "end" 67 | ] 68 | }, 69 | { 70 | "cell_type": "code", 71 | "execution_count": null, 72 | "metadata": {}, 73 | "outputs": [], 74 | "source": [ 75 | "#jump map\n", 76 | "function jump(x)\n", 77 | " #Flip sign of vertical component of velocity and multiply by coefficient of restitution\n", 78 | " γ = 0.9\n", 79 | " xn = [x[1]; 0.0; x[3]; -γ*x[4]]\n", 80 | "end" 81 | ] 82 | }, 83 | { 84 | "cell_type": "code", 85 | "execution_count": null, 86 | "metadata": {}, 87 | "outputs": [], 88 | "source": [ 89 | "#Initial Conditions\n", 90 | "r0 = [0; 1.0]\n", 91 | "v0 = [1.0; 0]\n", 92 | "x0 = [r0; v0];" 93 | ] 94 | }, 95 | { 96 | "cell_type": "code", 97 | "execution_count": null, 98 | "metadata": {}, 99 | "outputs": [], 100 | "source": [ 101 | "#Simulate\n", 102 | "Nx = 4\n", 103 | "h = 0.01 #100 Hz\n", 104 | "Tfinal = 3.0 # final time\n", 105 | "Nt = Int(Tfinal/h)+1 # number of time steps\n", 106 | "thist = Array(range(0,h*(Nt-1), step=h));\n", 107 | "xhist = zeros(Nx,Nt)\n", 108 | "xhist[:,1] = x0\n", 109 | "for k = 1:(Nt-1)\n", 110 | " xhist[:,k+1] = dynamics_rk4(xhist[:,k])\n", 111 | " if guard(xhist[:,k+1]) <= 0\n", 112 | " #interpolate back to guard=0\n", 113 | " \n", 114 | " xhist[:,k+1] = jump(xhist[:,k+1])\n", 115 | " end \n", 116 | "end" 117 | ] 118 | }, 119 | { 120 | "cell_type": "code", 121 | "execution_count": null, 122 | "metadata": {}, 123 | "outputs": [], 124 | "source": [ 125 | "plot(xhist[1,:],xhist[2,:])" 126 | ] 127 | }, 128 | { 129 | "cell_type": "code", 130 | "execution_count": null, 131 | "metadata": {}, 132 | "outputs": [], 133 | "source": [ 134 | "plot(xhist[4,:])" 135 | ] 136 | }, 137 | { 138 | "cell_type": "code", 139 | "execution_count": null, 140 | "metadata": {}, 141 | "outputs": [], 142 | "source": [] 143 | }, 144 | { 145 | "cell_type": "code", 146 | "execution_count": null, 147 | "metadata": {}, 148 | "outputs": [], 149 | "source": [] 150 | } 151 | ], 152 | "metadata": { 153 | "kernelspec": { 154 | "display_name": "Julia 1.6.5", 155 | "language": "julia", 156 | "name": "julia-1.6" 157 | }, 158 | "language_info": { 159 | "file_extension": ".jl", 160 | "mimetype": "application/julia", 161 | "name": "julia", 162 | "version": "1.6.5" 163 | } 164 | }, 165 | "nbformat": 4, 166 | "nbformat_minor": 4 167 | } 168 | -------------------------------------------------------------------------------- /Lecture 17/Lecture 17.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 17/Lecture 17.pdf -------------------------------------------------------------------------------- /Lecture 18/Lecture 18.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 18/Lecture 18.pdf -------------------------------------------------------------------------------- /Lecture 18/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 18/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 18/Project.toml -------------------------------------------------------------------------------- /Lecture 19/Lecture 19.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 19/Lecture 19.pdf -------------------------------------------------------------------------------- /Lecture 19/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 19/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 19/Project.toml -------------------------------------------------------------------------------- /Lecture 19/lqg.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "f8a3bd70-079d-42e5-925f-0328a8657725", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 11 | ] 12 | }, 13 | { 14 | "cell_type": "code", 15 | "execution_count": null, 16 | "id": "bc71ef22-6de2-4f40-98ce-3d933dfc7106", 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "using LinearAlgebra\n", 21 | "using PyPlot\n", 22 | "using SparseArrays\n", 23 | "using ControlSystems\n", 24 | "using ForwardDiff" 25 | ] 26 | }, 27 | { 28 | "cell_type": "code", 29 | "execution_count": null, 30 | "id": "7e13e806-4bac-4fa0-ab63-679aaa53643b", 31 | "metadata": {}, 32 | "outputs": [], 33 | "source": [ 34 | "# Discrete dynamics\n", 35 | "h = 0.1 # time step\n", 36 | "A = [1 h; 0 1]\n", 37 | "B = [0.5*h*h; h]\n", 38 | "C = [1.0 0]" 39 | ] 40 | }, 41 | { 42 | "cell_type": "code", 43 | "execution_count": null, 44 | "id": "5e3da132-9212-49a2-9d85-8199beae0eab", 45 | "metadata": {}, 46 | "outputs": [], 47 | "source": [ 48 | "# Noise Covariances\n", 49 | "W = B*0.1*B' #Corresponds to white noise force input to dynamics\n", 50 | "V = 0.1 #Noise on position measurements" 51 | ] 52 | }, 53 | { 54 | "cell_type": "code", 55 | "execution_count": null, 56 | "id": "87a50558-c7a0-435e-83ab-c982ceba8bf9", 57 | "metadata": {}, 58 | "outputs": [], 59 | "source": [ 60 | "n = 2 # number of state\n", 61 | "m = 1 # number of controls\n", 62 | "Tfinal = 10.0 # final time\n", 63 | "N = Int(Tfinal/h)+1 # number of time steps\n", 64 | "thist = Array(range(0,h*(N-1), step=h));" 65 | ] 66 | }, 67 | { 68 | "cell_type": "code", 69 | "execution_count": null, 70 | "id": "55ec4049-4cdb-47ca-b37d-9e402f18ad34", 71 | "metadata": {}, 72 | "outputs": [], 73 | "source": [ 74 | "# LQR Cost weights\n", 75 | "Q = Array(1.0*I(n))\n", 76 | "R = Array(0.1*I(m))\n", 77 | "Qn = Q" 78 | ] 79 | }, 80 | { 81 | "cell_type": "code", 82 | "execution_count": null, 83 | "id": "41cbec2b-233b-47ad-9ff1-f16bc7a4207c", 84 | "metadata": {}, 85 | "outputs": [], 86 | "source": [ 87 | "#Cost function\n", 88 | "function J(xhist,uhist)\n", 89 | " cost = 0.5*xhist[:,end]'*Qn*xhist[:,end]\n", 90 | " for k = 1:(size(xhist,2)-1)\n", 91 | " cost = cost + 0.5*xhist[:,k]'*Q*xhist[:,k] + 0.5*(uhist[k]'*R*uhist[k])[1]\n", 92 | " end\n", 93 | " return cost\n", 94 | "end" 95 | ] 96 | }, 97 | { 98 | "cell_type": "code", 99 | "execution_count": null, 100 | "id": "05bafaba-dd76-4f86-8a16-b9cb07f0cb97", 101 | "metadata": {}, 102 | "outputs": [], 103 | "source": [ 104 | "#Infinite-Horizon LQR Gain and cost-to-go\n", 105 | "P = dare(A,B,Q,R)\n", 106 | "K = dlqr(A,B,Q,R)" 107 | ] 108 | }, 109 | { 110 | "cell_type": "code", 111 | "execution_count": null, 112 | "id": "0ce5e70f-c45d-486b-9a99-3cd4c534132f", 113 | "metadata": {}, 114 | "outputs": [], 115 | "source": [ 116 | "# Initial conditions\n", 117 | "x0 = [1.0; 0] #True state\n", 118 | "\n", 119 | "#Filter state\n", 120 | "Σ0 = Array(1.0*I(2))\n", 121 | "x̂0 = [0.0; 0.0]" 122 | ] 123 | }, 124 | { 125 | "cell_type": "code", 126 | "execution_count": null, 127 | "id": "3a91498c-c9f3-44ee-b183-b332fe6c8ffc", 128 | "metadata": {}, 129 | "outputs": [], 130 | "source": [ 131 | "xhist = zeros(n,N)\n", 132 | "xhist[:,1] .= x0;\n", 133 | "\n", 134 | "uhist = zeros(N)\n", 135 | "yhist = zeros(N)\n", 136 | "x̂hist = zeros(n,N)\n", 137 | "Σhist = zeros(n,n,N);" 138 | ] 139 | }, 140 | { 141 | "cell_type": "code", 142 | "execution_count": null, 143 | "id": "dbd85ccc-42e1-4b60-86e8-0797301744a5", 144 | "metadata": {}, 145 | "outputs": [], 146 | "source": [ 147 | "#Initial Time Step\n", 148 | "\n", 149 | "#Generate Measurement\n", 150 | "yhist[1] = (C*xhist[:,1])[1] + sqrt(V)*randn()\n", 151 | "\n", 152 | "z = yhist[1] - (C*x̂0)[1] #Innovation\n", 153 | "S = (C*Σ0*C')[1] + V #Innovation Covariance\n", 154 | "\n", 155 | "L = Σ0*C'*inv(S) #Kalman Gain\n", 156 | "\n", 157 | "x̂hist[:,1] = x̂0 + L*z\n", 158 | "Σhist[:,:,1] .= (I-L*C)*Σ0*(I-L*C)' + L*V*L'\n", 159 | "\n", 160 | "uhist[1] = -(K*x̂hist[:,1])[1] #Control input\n", 161 | "\n", 162 | "xhist[:,2] .= A*xhist[:,1] + B*uhist[1] + sqrt(W)*randn(n) #Simulate with stochastic dynamics" 163 | ] 164 | }, 165 | { 166 | "cell_type": "code", 167 | "execution_count": null, 168 | "id": "6dae90a3-948a-40de-9708-cd5f51c1bd46", 169 | "metadata": {}, 170 | "outputs": [], 171 | "source": [ 172 | "for k = 2:(N-1)\n", 173 | " \n", 174 | " #Generate measurement\n", 175 | " yhist[k] = (C*xhist[:,k])[1] + sqrt(V)*randn()\n", 176 | "\n", 177 | " #KF Update\n", 178 | " x̃ = A*x̂hist[:,k-1] + B*uhist[k-1] #State Prediction\n", 179 | " Σ̃ = A*Σhist[:,:,k-1]*A' + W #Covariance Prediction\n", 180 | "\n", 181 | " z = yhist[k] - (C*x̃)[1] #Innovation\n", 182 | " S = (C*Σ̃*C')[1] + V #Innovation Covariance\n", 183 | "\n", 184 | " L = Σ̃*C'*inv(S) #Kalman Gain\n", 185 | "\n", 186 | " x̂hist[:,k] = x̃ + L*z\n", 187 | " Σhist[:,:,k] = (I-L*C)*Σ̃*(I-L*C)' + L*V*L'\n", 188 | "\n", 189 | " #LQR Controller\n", 190 | " uhist[k] = -(K*x̂hist[:,k])[1]\n", 191 | " \n", 192 | " #Run this on the stochastic dynamics\n", 193 | " xhist[:,k+1] .= A*xhist[:,k] + B*uhist[k] + sqrt(W)*randn(n)\n", 194 | "end" 195 | ] 196 | }, 197 | { 198 | "cell_type": "code", 199 | "execution_count": null, 200 | "id": "b80c5056-0361-4be0-9cc8-f89094aecbb6", 201 | "metadata": {}, 202 | "outputs": [], 203 | "source": [ 204 | "plot(xhist[1,:])" 205 | ] 206 | }, 207 | { 208 | "cell_type": "code", 209 | "execution_count": null, 210 | "id": "90ea7b22-87e8-4775-be5f-dac456e59399", 211 | "metadata": {}, 212 | "outputs": [], 213 | "source": [ 214 | "plot(x̂hist[1,:])" 215 | ] 216 | }, 217 | { 218 | "cell_type": "code", 219 | "execution_count": null, 220 | "id": "6fe7606d-3ba8-48a0-84c7-26890e2d393e", 221 | "metadata": {}, 222 | "outputs": [], 223 | "source": [ 224 | "#Covariance and Kalman gain converge to steady-state values in the LTI case\n", 225 | "#(just like LQR gain + cost-to-go Hessian)\n", 226 | "plot(Σhist[1,1,1:N-1])\n", 227 | "plot(Σhist[2,2,1:N-1])\n", 228 | "plot(Σhist[1,2,1:N-1])" 229 | ] 230 | }, 231 | { 232 | "cell_type": "code", 233 | "execution_count": null, 234 | "id": "071f1ac5-b1d0-43d2-9739-5cd790f9a42c", 235 | "metadata": {}, 236 | "outputs": [], 237 | "source": [] 238 | }, 239 | { 240 | "cell_type": "code", 241 | "execution_count": null, 242 | "id": "dd7d1415-a5f6-40b7-8211-0977222630c3", 243 | "metadata": {}, 244 | "outputs": [], 245 | "source": [] 246 | } 247 | ], 248 | "metadata": { 249 | "kernelspec": { 250 | "display_name": "Julia 1.6.5", 251 | "language": "julia", 252 | "name": "julia-1.6" 253 | }, 254 | "language_info": { 255 | "file_extension": ".jl", 256 | "mimetype": "application/julia", 257 | "name": "julia", 258 | "version": "1.6.5" 259 | } 260 | }, 261 | "nbformat": 4, 262 | "nbformat_minor": 5 263 | } 264 | -------------------------------------------------------------------------------- /Lecture 2/Lecture 2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 2/Lecture 2.pdf -------------------------------------------------------------------------------- /Lecture 2/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | [[ArgTools]] 4 | uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f" 5 | 6 | [[Artifacts]] 7 | uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33" 8 | 9 | [[Base64]] 10 | uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" 11 | 12 | [[ChainRulesCore]] 13 | deps = ["Compat", "LinearAlgebra", "SparseArrays"] 14 | git-tree-sha1 = "54fc4400de6e5c3e27be6047da2ef6ba355511f8" 15 | uuid = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" 16 | version = "1.11.6" 17 | 18 | [[ChangesOfVariables]] 19 | deps = ["ChainRulesCore", "LinearAlgebra", "Test"] 20 | git-tree-sha1 = "bf98fa45a0a4cee295de98d4c1462be26345b9a1" 21 | uuid = "9e997f8a-9a97-42d5-a9f1-ce6bfc15e2c0" 22 | version = "0.1.2" 23 | 24 | [[CommonSubexpressions]] 25 | deps = ["MacroTools", "Test"] 26 | git-tree-sha1 = "7b8a93dba8af7e3b42fecabf646260105ac373f7" 27 | uuid = "bbf7d656-a473-5ed7-a52c-81e309532950" 28 | version = "0.3.0" 29 | 30 | [[Compat]] 31 | deps = ["Base64", "Dates", "DelimitedFiles", "Distributed", "InteractiveUtils", "LibGit2", "Libdl", "LinearAlgebra", "Markdown", "Mmap", "Pkg", "Printf", "REPL", "Random", "SHA", "Serialization", "SharedArrays", "Sockets", "SparseArrays", "Statistics", "Test", "UUIDs", "Unicode"] 32 | git-tree-sha1 = "44c37b4636bc54afac5c574d2d02b625349d6582" 33 | uuid = "34da2185-b29b-5c13-b0c7-acf172513d20" 34 | version = "3.41.0" 35 | 36 | [[CompilerSupportLibraries_jll]] 37 | deps = ["Artifacts", "Libdl"] 38 | uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" 39 | 40 | [[Dates]] 41 | deps = ["Printf"] 42 | uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" 43 | 44 | [[DelimitedFiles]] 45 | deps = ["Mmap"] 46 | uuid = "8bb1440f-4735-579b-a4ab-409b98df4dab" 47 | 48 | [[DiffResults]] 49 | deps = ["StaticArrays"] 50 | git-tree-sha1 = "c18e98cba888c6c25d1c3b048e4b3380ca956805" 51 | uuid = "163ba53b-c6d8-5494-b064-1a9d43ac40c5" 52 | version = "1.0.3" 53 | 54 | [[DiffRules]] 55 | deps = ["LogExpFunctions", "NaNMath", "Random", "SpecialFunctions"] 56 | git-tree-sha1 = "9bc5dac3c8b6706b58ad5ce24cffd9861f07c94f" 57 | uuid = "b552c78f-8df3-52c6-915a-8e097449b14b" 58 | version = "1.9.0" 59 | 60 | [[Distributed]] 61 | deps = ["Random", "Serialization", "Sockets"] 62 | uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b" 63 | 64 | [[DocStringExtensions]] 65 | deps = ["LibGit2"] 66 | git-tree-sha1 = "b19534d1895d702889b219c382a6e18010797f0b" 67 | uuid = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" 68 | version = "0.8.6" 69 | 70 | [[Downloads]] 71 | deps = ["ArgTools", "LibCURL", "NetworkOptions"] 72 | uuid = "f43a241f-c20a-4ad4-852c-f6b1247861c6" 73 | 74 | [[ForwardDiff]] 75 | deps = ["CommonSubexpressions", "DiffResults", "DiffRules", "LinearAlgebra", "LogExpFunctions", "NaNMath", "Preferences", "Printf", "Random", "SpecialFunctions", "StaticArrays"] 76 | git-tree-sha1 = "1bd6fc0c344fc0cbee1f42f8d2e7ec8253dda2d2" 77 | uuid = "f6369f11-7733-5829-9624-2563aa707210" 78 | version = "0.10.25" 79 | 80 | [[InteractiveUtils]] 81 | deps = ["Markdown"] 82 | uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" 83 | 84 | [[InverseFunctions]] 85 | deps = ["Test"] 86 | git-tree-sha1 = "a7254c0acd8e62f1ac75ad24d5db43f5f19f3c65" 87 | uuid = "3587e190-3f89-42d0-90ee-14403ec27112" 88 | version = "0.1.2" 89 | 90 | [[IrrationalConstants]] 91 | git-tree-sha1 = "7fd44fd4ff43fc60815f8e764c0f352b83c49151" 92 | uuid = "92d709cd-6900-40b7-9082-c6be49f344b6" 93 | version = "0.1.1" 94 | 95 | [[JLLWrappers]] 96 | deps = ["Preferences"] 97 | git-tree-sha1 = "22df5b96feef82434b07327e2d3c770a9b21e023" 98 | uuid = "692b3bcd-3c85-4b1f-b108-f13ce0eb3210" 99 | version = "1.4.0" 100 | 101 | [[LibCURL]] 102 | deps = ["LibCURL_jll", "MozillaCACerts_jll"] 103 | uuid = "b27032c2-a3e7-50c8-80cd-2d36dbcbfd21" 104 | 105 | [[LibCURL_jll]] 106 | deps = ["Artifacts", "LibSSH2_jll", "Libdl", "MbedTLS_jll", "Zlib_jll", "nghttp2_jll"] 107 | uuid = "deac9b47-8bc7-5906-a0fe-35ac56dc84c0" 108 | 109 | [[LibGit2]] 110 | deps = ["Base64", "NetworkOptions", "Printf", "SHA"] 111 | uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" 112 | 113 | [[LibSSH2_jll]] 114 | deps = ["Artifacts", "Libdl", "MbedTLS_jll"] 115 | uuid = "29816b5a-b9ab-546f-933c-edad1886dfa8" 116 | 117 | [[Libdl]] 118 | uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" 119 | 120 | [[LinearAlgebra]] 121 | deps = ["Libdl"] 122 | uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 123 | 124 | [[LogExpFunctions]] 125 | deps = ["ChainRulesCore", "ChangesOfVariables", "DocStringExtensions", "InverseFunctions", "IrrationalConstants", "LinearAlgebra"] 126 | git-tree-sha1 = "e5718a00af0ab9756305a0392832c8952c7426c1" 127 | uuid = "2ab3a3ac-af41-5b50-aa03-7779005ae688" 128 | version = "0.3.6" 129 | 130 | [[Logging]] 131 | uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" 132 | 133 | [[MacroTools]] 134 | deps = ["Markdown", "Random"] 135 | git-tree-sha1 = "3d3e902b31198a27340d0bf00d6ac452866021cf" 136 | uuid = "1914dd2f-81c6-5fcd-8719-6d5c9610ff09" 137 | version = "0.5.9" 138 | 139 | [[Markdown]] 140 | deps = ["Base64"] 141 | uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" 142 | 143 | [[MbedTLS_jll]] 144 | deps = ["Artifacts", "Libdl"] 145 | uuid = "c8ffd9c3-330d-5841-b78e-0817d7145fa1" 146 | 147 | [[Mmap]] 148 | uuid = "a63ad114-7e13-5084-954f-fe012c677804" 149 | 150 | [[MozillaCACerts_jll]] 151 | uuid = "14a3606d-f60d-562e-9121-12d972cd8159" 152 | 153 | [[NaNMath]] 154 | git-tree-sha1 = "f755f36b19a5116bb580de457cda0c140153f283" 155 | uuid = "77ba4419-2d1f-58cd-9bb1-8ffee604a2e3" 156 | version = "0.3.6" 157 | 158 | [[NetworkOptions]] 159 | uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908" 160 | 161 | [[OpenLibm_jll]] 162 | deps = ["Artifacts", "Libdl"] 163 | uuid = "05823500-19ac-5b8b-9628-191a04bc5112" 164 | 165 | [[OpenSpecFun_jll]] 166 | deps = ["Artifacts", "CompilerSupportLibraries_jll", "JLLWrappers", "Libdl", "Pkg"] 167 | git-tree-sha1 = "13652491f6856acfd2db29360e1bbcd4565d04f1" 168 | uuid = "efe28fd5-8261-553b-a9e1-b2916fc3738e" 169 | version = "0.5.5+0" 170 | 171 | [[Pkg]] 172 | deps = ["Artifacts", "Dates", "Downloads", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "Serialization", "TOML", "Tar", "UUIDs", "p7zip_jll"] 173 | uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" 174 | 175 | [[Preferences]] 176 | deps = ["TOML"] 177 | git-tree-sha1 = "2cf929d64681236a2e074ffafb8d568733d2e6af" 178 | uuid = "21216c6a-2e73-6563-6e65-726566657250" 179 | version = "1.2.3" 180 | 181 | [[Printf]] 182 | deps = ["Unicode"] 183 | uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" 184 | 185 | [[REPL]] 186 | deps = ["InteractiveUtils", "Markdown", "Sockets", "Unicode"] 187 | uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" 188 | 189 | [[Random]] 190 | deps = ["Serialization"] 191 | uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" 192 | 193 | [[SHA]] 194 | uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" 195 | 196 | [[Serialization]] 197 | uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" 198 | 199 | [[SharedArrays]] 200 | deps = ["Distributed", "Mmap", "Random", "Serialization"] 201 | uuid = "1a1011a3-84de-559e-8e89-a11a2f7dc383" 202 | 203 | [[Sockets]] 204 | uuid = "6462fe0b-24de-5631-8697-dd941f90decc" 205 | 206 | [[SparseArrays]] 207 | deps = ["LinearAlgebra", "Random"] 208 | uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" 209 | 210 | [[SpecialFunctions]] 211 | deps = ["ChainRulesCore", "IrrationalConstants", "LogExpFunctions", "OpenLibm_jll", "OpenSpecFun_jll"] 212 | git-tree-sha1 = "e08890d19787ec25029113e88c34ec20cac1c91e" 213 | uuid = "276daf66-3868-5448-9aa4-cd146d93841b" 214 | version = "2.0.0" 215 | 216 | [[StaticArrays]] 217 | deps = ["LinearAlgebra", "Random", "Statistics"] 218 | git-tree-sha1 = "2884859916598f974858ff01df7dfc6c708dd895" 219 | uuid = "90137ffa-7385-5640-81b9-e52037218182" 220 | version = "1.3.3" 221 | 222 | [[Statistics]] 223 | deps = ["LinearAlgebra", "SparseArrays"] 224 | uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" 225 | 226 | [[TOML]] 227 | deps = ["Dates"] 228 | uuid = "fa267f1f-6049-4f14-aa54-33bafae1ed76" 229 | 230 | [[Tar]] 231 | deps = ["ArgTools", "SHA"] 232 | uuid = "a4e569a6-e804-4fa4-b0f3-eef7a1d5b13e" 233 | 234 | [[Test]] 235 | deps = ["InteractiveUtils", "Logging", "Random", "Serialization"] 236 | uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40" 237 | 238 | [[UUIDs]] 239 | deps = ["Random", "SHA"] 240 | uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" 241 | 242 | [[Unicode]] 243 | uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" 244 | 245 | [[Zlib_jll]] 246 | deps = ["Libdl"] 247 | uuid = "83775a58-1f1d-513f-b197-d71354ab007a" 248 | 249 | [[nghttp2_jll]] 250 | deps = ["Artifacts", "Libdl"] 251 | uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d" 252 | 253 | [[p7zip_jll]] 254 | deps = ["Artifacts", "Libdl"] 255 | uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0" 256 | -------------------------------------------------------------------------------- /Lecture 2/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" 3 | -------------------------------------------------------------------------------- /Lecture 20/Lecture 20.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 20/Lecture 20.pdf -------------------------------------------------------------------------------- /Lecture 21/Lecture 21.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 21/Lecture 21.pdf -------------------------------------------------------------------------------- /Lecture 22/Lecture 22.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 22/Lecture 22.pdf -------------------------------------------------------------------------------- /Lecture 23/Lecture 23.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 23/Lecture 23.pdf -------------------------------------------------------------------------------- /Lecture 24/Lecture 24.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 24/Lecture 24.pdf -------------------------------------------------------------------------------- /Lecture 3/Lecture 3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 3/Lecture 3.pdf -------------------------------------------------------------------------------- /Lecture 3/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | [[ArgTools]] 4 | uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f" 5 | 6 | [[Artifacts]] 7 | uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33" 8 | 9 | [[Base64]] 10 | uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" 11 | 12 | [[ChainRulesCore]] 13 | deps = ["Compat", "LinearAlgebra", "SparseArrays"] 14 | git-tree-sha1 = "54fc4400de6e5c3e27be6047da2ef6ba355511f8" 15 | uuid = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" 16 | version = "1.11.6" 17 | 18 | [[ChangesOfVariables]] 19 | deps = ["ChainRulesCore", "LinearAlgebra", "Test"] 20 | git-tree-sha1 = "bf98fa45a0a4cee295de98d4c1462be26345b9a1" 21 | uuid = "9e997f8a-9a97-42d5-a9f1-ce6bfc15e2c0" 22 | version = "0.1.2" 23 | 24 | [[ColorTypes]] 25 | deps = ["FixedPointNumbers", "Random"] 26 | git-tree-sha1 = "024fe24d83e4a5bf5fc80501a314ce0d1aa35597" 27 | uuid = "3da002f7-5984-5a60-b8a6-cbb66c0b333f" 28 | version = "0.11.0" 29 | 30 | [[Colors]] 31 | deps = ["ColorTypes", "FixedPointNumbers", "Reexport"] 32 | git-tree-sha1 = "417b0ed7b8b838aa6ca0a87aadf1bb9eb111ce40" 33 | uuid = "5ae59095-9a9b-59fe-a467-6f913c188581" 34 | version = "0.12.8" 35 | 36 | [[CommonSubexpressions]] 37 | deps = ["MacroTools", "Test"] 38 | git-tree-sha1 = "7b8a93dba8af7e3b42fecabf646260105ac373f7" 39 | uuid = "bbf7d656-a473-5ed7-a52c-81e309532950" 40 | version = "0.3.0" 41 | 42 | [[Compat]] 43 | deps = ["Base64", "Dates", "DelimitedFiles", "Distributed", "InteractiveUtils", "LibGit2", "Libdl", "LinearAlgebra", "Markdown", "Mmap", "Pkg", "Printf", "REPL", "Random", "SHA", "Serialization", "SharedArrays", "Sockets", "SparseArrays", "Statistics", "Test", "UUIDs", "Unicode"] 44 | git-tree-sha1 = "44c37b4636bc54afac5c574d2d02b625349d6582" 45 | uuid = "34da2185-b29b-5c13-b0c7-acf172513d20" 46 | version = "3.41.0" 47 | 48 | [[CompilerSupportLibraries_jll]] 49 | deps = ["Artifacts", "Libdl"] 50 | uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" 51 | 52 | [[Conda]] 53 | deps = ["Downloads", "JSON", "VersionParsing"] 54 | git-tree-sha1 = "6cdc8832ba11c7695f494c9d9a1c31e90959ce0f" 55 | uuid = "8f4d0f93-b110-5947-807f-2305c1781a2d" 56 | version = "1.6.0" 57 | 58 | [[Dates]] 59 | deps = ["Printf"] 60 | uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" 61 | 62 | [[DelimitedFiles]] 63 | deps = ["Mmap"] 64 | uuid = "8bb1440f-4735-579b-a4ab-409b98df4dab" 65 | 66 | [[DiffResults]] 67 | deps = ["StaticArrays"] 68 | git-tree-sha1 = "c18e98cba888c6c25d1c3b048e4b3380ca956805" 69 | uuid = "163ba53b-c6d8-5494-b064-1a9d43ac40c5" 70 | version = "1.0.3" 71 | 72 | [[DiffRules]] 73 | deps = ["LogExpFunctions", "NaNMath", "Random", "SpecialFunctions"] 74 | git-tree-sha1 = "9bc5dac3c8b6706b58ad5ce24cffd9861f07c94f" 75 | uuid = "b552c78f-8df3-52c6-915a-8e097449b14b" 76 | version = "1.9.0" 77 | 78 | [[Distributed]] 79 | deps = ["Random", "Serialization", "Sockets"] 80 | uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b" 81 | 82 | [[DocStringExtensions]] 83 | deps = ["LibGit2"] 84 | git-tree-sha1 = "b19534d1895d702889b219c382a6e18010797f0b" 85 | uuid = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" 86 | version = "0.8.6" 87 | 88 | [[Downloads]] 89 | deps = ["ArgTools", "LibCURL", "NetworkOptions"] 90 | uuid = "f43a241f-c20a-4ad4-852c-f6b1247861c6" 91 | 92 | [[FixedPointNumbers]] 93 | deps = ["Statistics"] 94 | git-tree-sha1 = "335bfdceacc84c5cdf16aadc768aa5ddfc5383cc" 95 | uuid = "53c48c17-4a7d-5ca2-90c5-79b7896eea93" 96 | version = "0.8.4" 97 | 98 | [[ForwardDiff]] 99 | deps = ["CommonSubexpressions", "DiffResults", "DiffRules", "LinearAlgebra", "LogExpFunctions", "NaNMath", "Preferences", "Printf", "Random", "SpecialFunctions", "StaticArrays"] 100 | git-tree-sha1 = "1bd6fc0c344fc0cbee1f42f8d2e7ec8253dda2d2" 101 | uuid = "f6369f11-7733-5829-9624-2563aa707210" 102 | version = "0.10.25" 103 | 104 | [[InteractiveUtils]] 105 | deps = ["Markdown"] 106 | uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" 107 | 108 | [[InverseFunctions]] 109 | deps = ["Test"] 110 | git-tree-sha1 = "a7254c0acd8e62f1ac75ad24d5db43f5f19f3c65" 111 | uuid = "3587e190-3f89-42d0-90ee-14403ec27112" 112 | version = "0.1.2" 113 | 114 | [[IrrationalConstants]] 115 | git-tree-sha1 = "7fd44fd4ff43fc60815f8e764c0f352b83c49151" 116 | uuid = "92d709cd-6900-40b7-9082-c6be49f344b6" 117 | version = "0.1.1" 118 | 119 | [[JLLWrappers]] 120 | deps = ["Preferences"] 121 | git-tree-sha1 = "22df5b96feef82434b07327e2d3c770a9b21e023" 122 | uuid = "692b3bcd-3c85-4b1f-b108-f13ce0eb3210" 123 | version = "1.4.0" 124 | 125 | [[JSON]] 126 | deps = ["Dates", "Mmap", "Parsers", "Unicode"] 127 | git-tree-sha1 = "8076680b162ada2a031f707ac7b4953e30667a37" 128 | uuid = "682c06a0-de6a-54ab-a142-c8b1cf79cde6" 129 | version = "0.21.2" 130 | 131 | [[LaTeXStrings]] 132 | git-tree-sha1 = "f2355693d6778a178ade15952b7ac47a4ff97996" 133 | uuid = "b964fa9f-0449-5b57-a5c2-d3ea65f4040f" 134 | version = "1.3.0" 135 | 136 | [[LibCURL]] 137 | deps = ["LibCURL_jll", "MozillaCACerts_jll"] 138 | uuid = "b27032c2-a3e7-50c8-80cd-2d36dbcbfd21" 139 | 140 | [[LibCURL_jll]] 141 | deps = ["Artifacts", "LibSSH2_jll", "Libdl", "MbedTLS_jll", "Zlib_jll", "nghttp2_jll"] 142 | uuid = "deac9b47-8bc7-5906-a0fe-35ac56dc84c0" 143 | 144 | [[LibGit2]] 145 | deps = ["Base64", "NetworkOptions", "Printf", "SHA"] 146 | uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" 147 | 148 | [[LibSSH2_jll]] 149 | deps = ["Artifacts", "Libdl", "MbedTLS_jll"] 150 | uuid = "29816b5a-b9ab-546f-933c-edad1886dfa8" 151 | 152 | [[Libdl]] 153 | uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" 154 | 155 | [[LinearAlgebra]] 156 | deps = ["Libdl"] 157 | uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 158 | 159 | [[LogExpFunctions]] 160 | deps = ["ChainRulesCore", "ChangesOfVariables", "DocStringExtensions", "InverseFunctions", "IrrationalConstants", "LinearAlgebra"] 161 | git-tree-sha1 = "e5718a00af0ab9756305a0392832c8952c7426c1" 162 | uuid = "2ab3a3ac-af41-5b50-aa03-7779005ae688" 163 | version = "0.3.6" 164 | 165 | [[Logging]] 166 | uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" 167 | 168 | [[MacroTools]] 169 | deps = ["Markdown", "Random"] 170 | git-tree-sha1 = "3d3e902b31198a27340d0bf00d6ac452866021cf" 171 | uuid = "1914dd2f-81c6-5fcd-8719-6d5c9610ff09" 172 | version = "0.5.9" 173 | 174 | [[Markdown]] 175 | deps = ["Base64"] 176 | uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" 177 | 178 | [[MbedTLS_jll]] 179 | deps = ["Artifacts", "Libdl"] 180 | uuid = "c8ffd9c3-330d-5841-b78e-0817d7145fa1" 181 | 182 | [[Mmap]] 183 | uuid = "a63ad114-7e13-5084-954f-fe012c677804" 184 | 185 | [[MozillaCACerts_jll]] 186 | uuid = "14a3606d-f60d-562e-9121-12d972cd8159" 187 | 188 | [[NaNMath]] 189 | git-tree-sha1 = "f755f36b19a5116bb580de457cda0c140153f283" 190 | uuid = "77ba4419-2d1f-58cd-9bb1-8ffee604a2e3" 191 | version = "0.3.6" 192 | 193 | [[NetworkOptions]] 194 | uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908" 195 | 196 | [[OpenLibm_jll]] 197 | deps = ["Artifacts", "Libdl"] 198 | uuid = "05823500-19ac-5b8b-9628-191a04bc5112" 199 | 200 | [[OpenSpecFun_jll]] 201 | deps = ["Artifacts", "CompilerSupportLibraries_jll", "JLLWrappers", "Libdl", "Pkg"] 202 | git-tree-sha1 = "13652491f6856acfd2db29360e1bbcd4565d04f1" 203 | uuid = "efe28fd5-8261-553b-a9e1-b2916fc3738e" 204 | version = "0.5.5+0" 205 | 206 | [[Parsers]] 207 | deps = ["Dates"] 208 | git-tree-sha1 = "92f91ba9e5941fc781fecf5494ac1da87bdac775" 209 | uuid = "69de0a69-1ddd-5017-9359-2bf0b02dc9f0" 210 | version = "2.2.0" 211 | 212 | [[Pkg]] 213 | deps = ["Artifacts", "Dates", "Downloads", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "Serialization", "TOML", "Tar", "UUIDs", "p7zip_jll"] 214 | uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" 215 | 216 | [[Preferences]] 217 | deps = ["TOML"] 218 | git-tree-sha1 = "2cf929d64681236a2e074ffafb8d568733d2e6af" 219 | uuid = "21216c6a-2e73-6563-6e65-726566657250" 220 | version = "1.2.3" 221 | 222 | [[Printf]] 223 | deps = ["Unicode"] 224 | uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" 225 | 226 | [[PyCall]] 227 | deps = ["Conda", "Dates", "Libdl", "LinearAlgebra", "MacroTools", "Serialization", "VersionParsing"] 228 | git-tree-sha1 = "71fd4022ecd0c6d20180e23ff1b3e05a143959c2" 229 | uuid = "438e738f-606a-5dbb-bf0a-cddfbfd45ab0" 230 | version = "1.93.0" 231 | 232 | [[PyPlot]] 233 | deps = ["Colors", "LaTeXStrings", "PyCall", "Sockets", "Test", "VersionParsing"] 234 | git-tree-sha1 = "14c1b795b9d764e1784713941e787e1384268103" 235 | uuid = "d330b81b-6aea-500a-939a-2ce795aea3ee" 236 | version = "2.10.0" 237 | 238 | [[REPL]] 239 | deps = ["InteractiveUtils", "Markdown", "Sockets", "Unicode"] 240 | uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" 241 | 242 | [[Random]] 243 | deps = ["Serialization"] 244 | uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" 245 | 246 | [[Reexport]] 247 | git-tree-sha1 = "45e428421666073eab6f2da5c9d310d99bb12f9b" 248 | uuid = "189a3867-3050-52da-a836-e630ba90ab69" 249 | version = "1.2.2" 250 | 251 | [[SHA]] 252 | uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" 253 | 254 | [[Serialization]] 255 | uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" 256 | 257 | [[SharedArrays]] 258 | deps = ["Distributed", "Mmap", "Random", "Serialization"] 259 | uuid = "1a1011a3-84de-559e-8e89-a11a2f7dc383" 260 | 261 | [[Sockets]] 262 | uuid = "6462fe0b-24de-5631-8697-dd941f90decc" 263 | 264 | [[SparseArrays]] 265 | deps = ["LinearAlgebra", "Random"] 266 | uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" 267 | 268 | [[SpecialFunctions]] 269 | deps = ["ChainRulesCore", "IrrationalConstants", "LogExpFunctions", "OpenLibm_jll", "OpenSpecFun_jll"] 270 | git-tree-sha1 = "e08890d19787ec25029113e88c34ec20cac1c91e" 271 | uuid = "276daf66-3868-5448-9aa4-cd146d93841b" 272 | version = "2.0.0" 273 | 274 | [[StaticArrays]] 275 | deps = ["LinearAlgebra", "Random", "Statistics"] 276 | git-tree-sha1 = "2884859916598f974858ff01df7dfc6c708dd895" 277 | uuid = "90137ffa-7385-5640-81b9-e52037218182" 278 | version = "1.3.3" 279 | 280 | [[Statistics]] 281 | deps = ["LinearAlgebra", "SparseArrays"] 282 | uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" 283 | 284 | [[TOML]] 285 | deps = ["Dates"] 286 | uuid = "fa267f1f-6049-4f14-aa54-33bafae1ed76" 287 | 288 | [[Tar]] 289 | deps = ["ArgTools", "SHA"] 290 | uuid = "a4e569a6-e804-4fa4-b0f3-eef7a1d5b13e" 291 | 292 | [[Test]] 293 | deps = ["InteractiveUtils", "Logging", "Random", "Serialization"] 294 | uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40" 295 | 296 | [[UUIDs]] 297 | deps = ["Random", "SHA"] 298 | uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" 299 | 300 | [[Unicode]] 301 | uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" 302 | 303 | [[VersionParsing]] 304 | git-tree-sha1 = "58d6e80b4ee071f5efd07fda82cb9fbe17200868" 305 | uuid = "81def892-9a0e-5fdd-b105-ffc91e053289" 306 | version = "1.3.0" 307 | 308 | [[Zlib_jll]] 309 | deps = ["Libdl"] 310 | uuid = "83775a58-1f1d-513f-b197-d71354ab007a" 311 | 312 | [[nghttp2_jll]] 313 | deps = ["Artifacts", "Libdl"] 314 | uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d" 315 | 316 | [[p7zip_jll]] 317 | deps = ["Artifacts", "Libdl"] 318 | uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0" 319 | -------------------------------------------------------------------------------- /Lecture 3/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" 3 | PyPlot = "d330b81b-6aea-500a-939a-2ce795aea3ee" 4 | -------------------------------------------------------------------------------- /Lecture 3/minimization.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using ForwardDiff\n", 20 | "using PyPlot" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "function f(x)\n", 30 | " return x.^4 + x.^3 - x.^2 - x\n", 31 | "end" 32 | ] 33 | }, 34 | { 35 | "cell_type": "code", 36 | "execution_count": null, 37 | "metadata": {}, 38 | "outputs": [], 39 | "source": [ 40 | "function ∇f(x)\n", 41 | " return 4.0*x.^3 + 3.0*x.^2 - 2.0*x - 1.0\n", 42 | "end" 43 | ] 44 | }, 45 | { 46 | "cell_type": "code", 47 | "execution_count": null, 48 | "metadata": {}, 49 | "outputs": [], 50 | "source": [ 51 | "function ∇2f(x)\n", 52 | " return 12.0*x.^2 + 6.0*x - 2.0\n", 53 | "end" 54 | ] 55 | }, 56 | { 57 | "cell_type": "code", 58 | "execution_count": null, 59 | "metadata": {}, 60 | "outputs": [], 61 | "source": [ 62 | "x = LinRange(-1.75,1.25,1000)" 63 | ] 64 | }, 65 | { 66 | "cell_type": "code", 67 | "execution_count": null, 68 | "metadata": {}, 69 | "outputs": [], 70 | "source": [ 71 | "p = plot(x,f(x))" 72 | ] 73 | }, 74 | { 75 | "cell_type": "code", 76 | "execution_count": null, 77 | "metadata": {}, 78 | "outputs": [], 79 | "source": [ 80 | "function newton_step(x0)\n", 81 | " xn = x0 - ∇2f(x0)\\∇f(x0)\n", 82 | "end" 83 | ] 84 | }, 85 | { 86 | "cell_type": "code", 87 | "execution_count": null, 88 | "metadata": {}, 89 | "outputs": [], 90 | "source": [ 91 | "xguess = 0.0\n", 92 | "plot(x, f(x))\n", 93 | "plot(xguess, f(xguess), \"rx\")" 94 | ] 95 | }, 96 | { 97 | "cell_type": "code", 98 | "execution_count": null, 99 | "metadata": {}, 100 | "outputs": [], 101 | "source": [ 102 | "xnew = newton_step(xguess[end])\n", 103 | "xguess = [xguess xnew]\n", 104 | "plot(x, f(x))\n", 105 | "plot(xguess, f(xguess), \"rx\")" 106 | ] 107 | }, 108 | { 109 | "cell_type": "code", 110 | "execution_count": null, 111 | "metadata": {}, 112 | "outputs": [], 113 | "source": [] 114 | }, 115 | { 116 | "cell_type": "code", 117 | "execution_count": null, 118 | "metadata": {}, 119 | "outputs": [], 120 | "source": [ 121 | "∇2f(0.0)" 122 | ] 123 | }, 124 | { 125 | "cell_type": "code", 126 | "execution_count": null, 127 | "metadata": {}, 128 | "outputs": [], 129 | "source": [ 130 | "function regularized_newton_step(x0)\n", 131 | " β = 1.0\n", 132 | " H = ∇2f(x0)\n", 133 | " while !isposdef(H)\n", 134 | " H = H + β*I\n", 135 | " end\n", 136 | " xn = x0 - H\\∇f(x0)\n", 137 | "end" 138 | ] 139 | }, 140 | { 141 | "cell_type": "code", 142 | "execution_count": null, 143 | "metadata": {}, 144 | "outputs": [], 145 | "source": [ 146 | "xguess = 0.0\n", 147 | "plot(x, f(x))\n", 148 | "plot(xguess, f(xguess), \"rx\")" 149 | ] 150 | }, 151 | { 152 | "cell_type": "code", 153 | "execution_count": null, 154 | "metadata": {}, 155 | "outputs": [], 156 | "source": [ 157 | "xnew = regularized_newton_step(xguess[end])\n", 158 | "xguess = [xguess xnew]\n", 159 | "plot(x, f(x))\n", 160 | "plot(xguess, f(xguess), \"rx\")" 161 | ] 162 | }, 163 | { 164 | "cell_type": "code", 165 | "execution_count": null, 166 | "metadata": {}, 167 | "outputs": [], 168 | "source": [] 169 | } 170 | ], 171 | "metadata": { 172 | "kernelspec": { 173 | "display_name": "Julia 1.6.5", 174 | "language": "julia", 175 | "name": "julia-1.6" 176 | }, 177 | "language_info": { 178 | "file_extension": ".jl", 179 | "mimetype": "application/julia", 180 | "name": "julia", 181 | "version": "1.6.5" 182 | } 183 | }, 184 | "nbformat": 4, 185 | "nbformat_minor": 4 186 | } 187 | -------------------------------------------------------------------------------- /Lecture 3/root-finding.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using ForwardDiff\n", 20 | "using PyPlot" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "function pendulum_dynamics(x)\n", 30 | " l = 1.0\n", 31 | " g = 9.81\n", 32 | " \n", 33 | " θ = x[1]\n", 34 | " θ̇ = x[2]\n", 35 | " \n", 36 | " θ̈ = -(g/l)*sin(θ)\n", 37 | " \n", 38 | " return [θ̇; θ̈]\n", 39 | "end" 40 | ] 41 | }, 42 | { 43 | "cell_type": "code", 44 | "execution_count": null, 45 | "metadata": {}, 46 | "outputs": [], 47 | "source": [ 48 | "function backward_euler_step_fixed_point(fun, x0, h)\n", 49 | " xn = x0\n", 50 | " e = [norm(x0 + h.*fun(xn) - xn)]\n", 51 | " while e[end] > 1e-8\n", 52 | " xn = x0 + h.*fun(xn)\n", 53 | " e = [e; norm(x0 + h.*fun(xn) - xn)]\n", 54 | " end\n", 55 | " \n", 56 | " return xn, e\n", 57 | "end" 58 | ] 59 | }, 60 | { 61 | "cell_type": "code", 62 | "execution_count": null, 63 | "metadata": {}, 64 | "outputs": [], 65 | "source": [ 66 | "function backward_euler_step_newton(fun, x0, h)\n", 67 | " xn = x0\n", 68 | " r = x0 + h.*fun(xn) - xn\n", 69 | " e = [norm(r)]\n", 70 | " while e[end] > 1e-8\n", 71 | " ∂r = ForwardDiff.jacobian(x -> x0 + h.*fun(x) - x, xn)\n", 72 | " xn = xn - ∂r\\r\n", 73 | " r = x0 + h.*fun(xn) - xn\n", 74 | " e = [e; norm(r)]\n", 75 | " end\n", 76 | " \n", 77 | " return xn, e\n", 78 | "end" 79 | ] 80 | }, 81 | { 82 | "cell_type": "code", 83 | "execution_count": null, 84 | "metadata": {}, 85 | "outputs": [], 86 | "source": [ 87 | "function backward_euler_fixed_point(fun, x0, Tf, h)\n", 88 | " t = Array(range(0,Tf,step=h))\n", 89 | " \n", 90 | " x_hist = zeros(length(x0),length(t))\n", 91 | " x_hist[:,1] .= x0\n", 92 | " \n", 93 | " for k = 1:(length(t)-1)\n", 94 | " x_hist[:,k+1], e = backward_euler_step_fixed_point(fun, x_hist[:,k], h)\n", 95 | " end\n", 96 | " \n", 97 | " return x_hist, t\n", 98 | "end" 99 | ] 100 | }, 101 | { 102 | "cell_type": "code", 103 | "execution_count": null, 104 | "metadata": {}, 105 | "outputs": [], 106 | "source": [ 107 | "function backward_euler_newton(fun, x0, Tf, h)\n", 108 | " t = Array(range(0,Tf,step=h))\n", 109 | " \n", 110 | " x_hist = zeros(length(x0),length(t))\n", 111 | " x_hist[:,1] .= x0\n", 112 | " \n", 113 | " for k = 1:(length(t)-1)\n", 114 | " x_hist[:,k+1], e = backward_euler_step_newton(fun, x_hist[:,k], h)\n", 115 | " end\n", 116 | " \n", 117 | " return x_hist, t\n", 118 | "end" 119 | ] 120 | }, 121 | { 122 | "cell_type": "code", 123 | "execution_count": null, 124 | "metadata": {}, 125 | "outputs": [], 126 | "source": [ 127 | "x0 = [.1; 0]\n", 128 | "x_hist1, t_hist1 = backward_euler_fixed_point(pendulum_dynamics, x0, 10, 0.01)\n", 129 | "x_hist2, t_hist2 = backward_euler_newton(pendulum_dynamics, x0, 10, 0.01)\n", 130 | "plot(t_hist1, x_hist1[1,:])\n", 131 | "plot(t_hist2, x_hist2[1,:])" 132 | ] 133 | }, 134 | { 135 | "cell_type": "code", 136 | "execution_count": null, 137 | "metadata": {}, 138 | "outputs": [], 139 | "source": [ 140 | "max(abs.(x_hist1-x_hist2)...)" 141 | ] 142 | }, 143 | { 144 | "cell_type": "code", 145 | "execution_count": null, 146 | "metadata": {}, 147 | "outputs": [], 148 | "source": [ 149 | "xn, e1 = backward_euler_step_fixed_point(pendulum_dynamics, x0, 0.1)\n", 150 | "e1" 151 | ] 152 | }, 153 | { 154 | "cell_type": "code", 155 | "execution_count": null, 156 | "metadata": {}, 157 | "outputs": [], 158 | "source": [ 159 | "xn, e2 = backward_euler_step_newton(pendulum_dynamics, x0, 0.1)\n", 160 | "e2" 161 | ] 162 | }, 163 | { 164 | "cell_type": "code", 165 | "execution_count": null, 166 | "metadata": {}, 167 | "outputs": [], 168 | "source": [ 169 | "semilogy(e1)\n", 170 | "semilogy(e2)" 171 | ] 172 | }, 173 | { 174 | "cell_type": "code", 175 | "execution_count": null, 176 | "metadata": {}, 177 | "outputs": [], 178 | "source": [] 179 | }, 180 | { 181 | "cell_type": "code", 182 | "execution_count": null, 183 | "metadata": {}, 184 | "outputs": [], 185 | "source": [] 186 | } 187 | ], 188 | "metadata": { 189 | "kernelspec": { 190 | "display_name": "Julia 1.6.5", 191 | "language": "julia", 192 | "name": "julia-1.6" 193 | }, 194 | "language_info": { 195 | "file_extension": ".jl", 196 | "mimetype": "application/julia", 197 | "name": "julia", 198 | "version": "1.6.5" 199 | } 200 | }, 201 | "nbformat": 4, 202 | "nbformat_minor": 4 203 | } 204 | -------------------------------------------------------------------------------- /Lecture 4/Lecture 4.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 4/Lecture 4.pdf -------------------------------------------------------------------------------- /Lecture 4/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | [[ArgTools]] 4 | uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f" 5 | 6 | [[Artifacts]] 7 | uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33" 8 | 9 | [[Base64]] 10 | uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" 11 | 12 | [[ChainRulesCore]] 13 | deps = ["Compat", "LinearAlgebra", "SparseArrays"] 14 | git-tree-sha1 = "54fc4400de6e5c3e27be6047da2ef6ba355511f8" 15 | uuid = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" 16 | version = "1.11.6" 17 | 18 | [[ChangesOfVariables]] 19 | deps = ["ChainRulesCore", "LinearAlgebra", "Test"] 20 | git-tree-sha1 = "bf98fa45a0a4cee295de98d4c1462be26345b9a1" 21 | uuid = "9e997f8a-9a97-42d5-a9f1-ce6bfc15e2c0" 22 | version = "0.1.2" 23 | 24 | [[CommonSubexpressions]] 25 | deps = ["MacroTools", "Test"] 26 | git-tree-sha1 = "7b8a93dba8af7e3b42fecabf646260105ac373f7" 27 | uuid = "bbf7d656-a473-5ed7-a52c-81e309532950" 28 | version = "0.3.0" 29 | 30 | [[Compat]] 31 | deps = ["Base64", "Dates", "DelimitedFiles", "Distributed", "InteractiveUtils", "LibGit2", "Libdl", "LinearAlgebra", "Markdown", "Mmap", "Pkg", "Printf", "REPL", "Random", "SHA", "Serialization", "SharedArrays", "Sockets", "SparseArrays", "Statistics", "Test", "UUIDs", "Unicode"] 32 | git-tree-sha1 = "44c37b4636bc54afac5c574d2d02b625349d6582" 33 | uuid = "34da2185-b29b-5c13-b0c7-acf172513d20" 34 | version = "3.41.0" 35 | 36 | [[CompilerSupportLibraries_jll]] 37 | deps = ["Artifacts", "Libdl"] 38 | uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" 39 | 40 | [[Dates]] 41 | deps = ["Printf"] 42 | uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" 43 | 44 | [[DelimitedFiles]] 45 | deps = ["Mmap"] 46 | uuid = "8bb1440f-4735-579b-a4ab-409b98df4dab" 47 | 48 | [[DiffResults]] 49 | deps = ["StaticArrays"] 50 | git-tree-sha1 = "c18e98cba888c6c25d1c3b048e4b3380ca956805" 51 | uuid = "163ba53b-c6d8-5494-b064-1a9d43ac40c5" 52 | version = "1.0.3" 53 | 54 | [[DiffRules]] 55 | deps = ["LogExpFunctions", "NaNMath", "Random", "SpecialFunctions"] 56 | git-tree-sha1 = "9bc5dac3c8b6706b58ad5ce24cffd9861f07c94f" 57 | uuid = "b552c78f-8df3-52c6-915a-8e097449b14b" 58 | version = "1.9.0" 59 | 60 | [[Distributed]] 61 | deps = ["Random", "Serialization", "Sockets"] 62 | uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b" 63 | 64 | [[DocStringExtensions]] 65 | deps = ["LibGit2"] 66 | git-tree-sha1 = "b19534d1895d702889b219c382a6e18010797f0b" 67 | uuid = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" 68 | version = "0.8.6" 69 | 70 | [[Downloads]] 71 | deps = ["ArgTools", "LibCURL", "NetworkOptions"] 72 | uuid = "f43a241f-c20a-4ad4-852c-f6b1247861c6" 73 | 74 | [[ForwardDiff]] 75 | deps = ["CommonSubexpressions", "DiffResults", "DiffRules", "LinearAlgebra", "LogExpFunctions", "NaNMath", "Preferences", "Printf", "Random", "SpecialFunctions", "StaticArrays"] 76 | git-tree-sha1 = "1bd6fc0c344fc0cbee1f42f8d2e7ec8253dda2d2" 77 | uuid = "f6369f11-7733-5829-9624-2563aa707210" 78 | version = "0.10.25" 79 | 80 | [[InteractiveUtils]] 81 | deps = ["Markdown"] 82 | uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" 83 | 84 | [[InverseFunctions]] 85 | deps = ["Test"] 86 | git-tree-sha1 = "a7254c0acd8e62f1ac75ad24d5db43f5f19f3c65" 87 | uuid = "3587e190-3f89-42d0-90ee-14403ec27112" 88 | version = "0.1.2" 89 | 90 | [[IrrationalConstants]] 91 | git-tree-sha1 = "7fd44fd4ff43fc60815f8e764c0f352b83c49151" 92 | uuid = "92d709cd-6900-40b7-9082-c6be49f344b6" 93 | version = "0.1.1" 94 | 95 | [[JLLWrappers]] 96 | deps = ["Preferences"] 97 | git-tree-sha1 = "22df5b96feef82434b07327e2d3c770a9b21e023" 98 | uuid = "692b3bcd-3c85-4b1f-b108-f13ce0eb3210" 99 | version = "1.4.0" 100 | 101 | [[LibCURL]] 102 | deps = ["LibCURL_jll", "MozillaCACerts_jll"] 103 | uuid = "b27032c2-a3e7-50c8-80cd-2d36dbcbfd21" 104 | 105 | [[LibCURL_jll]] 106 | deps = ["Artifacts", "LibSSH2_jll", "Libdl", "MbedTLS_jll", "Zlib_jll", "nghttp2_jll"] 107 | uuid = "deac9b47-8bc7-5906-a0fe-35ac56dc84c0" 108 | 109 | [[LibGit2]] 110 | deps = ["Base64", "NetworkOptions", "Printf", "SHA"] 111 | uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" 112 | 113 | [[LibSSH2_jll]] 114 | deps = ["Artifacts", "Libdl", "MbedTLS_jll"] 115 | uuid = "29816b5a-b9ab-546f-933c-edad1886dfa8" 116 | 117 | [[Libdl]] 118 | uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" 119 | 120 | [[LinearAlgebra]] 121 | deps = ["Libdl"] 122 | uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 123 | 124 | [[LogExpFunctions]] 125 | deps = ["ChainRulesCore", "ChangesOfVariables", "DocStringExtensions", "InverseFunctions", "IrrationalConstants", "LinearAlgebra"] 126 | git-tree-sha1 = "e5718a00af0ab9756305a0392832c8952c7426c1" 127 | uuid = "2ab3a3ac-af41-5b50-aa03-7779005ae688" 128 | version = "0.3.6" 129 | 130 | [[Logging]] 131 | uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" 132 | 133 | [[MacroTools]] 134 | deps = ["Markdown", "Random"] 135 | git-tree-sha1 = "3d3e902b31198a27340d0bf00d6ac452866021cf" 136 | uuid = "1914dd2f-81c6-5fcd-8719-6d5c9610ff09" 137 | version = "0.5.9" 138 | 139 | [[Markdown]] 140 | deps = ["Base64"] 141 | uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" 142 | 143 | [[MbedTLS_jll]] 144 | deps = ["Artifacts", "Libdl"] 145 | uuid = "c8ffd9c3-330d-5841-b78e-0817d7145fa1" 146 | 147 | [[Mmap]] 148 | uuid = "a63ad114-7e13-5084-954f-fe012c677804" 149 | 150 | [[MozillaCACerts_jll]] 151 | uuid = "14a3606d-f60d-562e-9121-12d972cd8159" 152 | 153 | [[NaNMath]] 154 | git-tree-sha1 = "f755f36b19a5116bb580de457cda0c140153f283" 155 | uuid = "77ba4419-2d1f-58cd-9bb1-8ffee604a2e3" 156 | version = "0.3.6" 157 | 158 | [[NetworkOptions]] 159 | uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908" 160 | 161 | [[OpenLibm_jll]] 162 | deps = ["Artifacts", "Libdl"] 163 | uuid = "05823500-19ac-5b8b-9628-191a04bc5112" 164 | 165 | [[OpenSpecFun_jll]] 166 | deps = ["Artifacts", "CompilerSupportLibraries_jll", "JLLWrappers", "Libdl", "Pkg"] 167 | git-tree-sha1 = "13652491f6856acfd2db29360e1bbcd4565d04f1" 168 | uuid = "efe28fd5-8261-553b-a9e1-b2916fc3738e" 169 | version = "0.5.5+0" 170 | 171 | [[Pkg]] 172 | deps = ["Artifacts", "Dates", "Downloads", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "Serialization", "TOML", "Tar", "UUIDs", "p7zip_jll"] 173 | uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" 174 | 175 | [[Preferences]] 176 | deps = ["TOML"] 177 | git-tree-sha1 = "2cf929d64681236a2e074ffafb8d568733d2e6af" 178 | uuid = "21216c6a-2e73-6563-6e65-726566657250" 179 | version = "1.2.3" 180 | 181 | [[Printf]] 182 | deps = ["Unicode"] 183 | uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" 184 | 185 | [[REPL]] 186 | deps = ["InteractiveUtils", "Markdown", "Sockets", "Unicode"] 187 | uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" 188 | 189 | [[Random]] 190 | deps = ["Serialization"] 191 | uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" 192 | 193 | [[SHA]] 194 | uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" 195 | 196 | [[Serialization]] 197 | uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" 198 | 199 | [[SharedArrays]] 200 | deps = ["Distributed", "Mmap", "Random", "Serialization"] 201 | uuid = "1a1011a3-84de-559e-8e89-a11a2f7dc383" 202 | 203 | [[Sockets]] 204 | uuid = "6462fe0b-24de-5631-8697-dd941f90decc" 205 | 206 | [[SparseArrays]] 207 | deps = ["LinearAlgebra", "Random"] 208 | uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" 209 | 210 | [[SpecialFunctions]] 211 | deps = ["ChainRulesCore", "IrrationalConstants", "LogExpFunctions", "OpenLibm_jll", "OpenSpecFun_jll"] 212 | git-tree-sha1 = "e6bf188613555c78062842777b116905a9f9dd49" 213 | uuid = "276daf66-3868-5448-9aa4-cd146d93841b" 214 | version = "2.1.0" 215 | 216 | [[StaticArrays]] 217 | deps = ["LinearAlgebra", "Random", "Statistics"] 218 | git-tree-sha1 = "2884859916598f974858ff01df7dfc6c708dd895" 219 | uuid = "90137ffa-7385-5640-81b9-e52037218182" 220 | version = "1.3.3" 221 | 222 | [[Statistics]] 223 | deps = ["LinearAlgebra", "SparseArrays"] 224 | uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" 225 | 226 | [[TOML]] 227 | deps = ["Dates"] 228 | uuid = "fa267f1f-6049-4f14-aa54-33bafae1ed76" 229 | 230 | [[Tar]] 231 | deps = ["ArgTools", "SHA"] 232 | uuid = "a4e569a6-e804-4fa4-b0f3-eef7a1d5b13e" 233 | 234 | [[Test]] 235 | deps = ["InteractiveUtils", "Logging", "Random", "Serialization"] 236 | uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40" 237 | 238 | [[UUIDs]] 239 | deps = ["Random", "SHA"] 240 | uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" 241 | 242 | [[Unicode]] 243 | uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" 244 | 245 | [[Zlib_jll]] 246 | deps = ["Libdl"] 247 | uuid = "83775a58-1f1d-513f-b197-d71354ab007a" 248 | 249 | [[nghttp2_jll]] 250 | deps = ["Artifacts", "Libdl"] 251 | uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d" 252 | 253 | [[p7zip_jll]] 254 | deps = ["Artifacts", "Libdl"] 255 | uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0" 256 | -------------------------------------------------------------------------------- /Lecture 4/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" 3 | -------------------------------------------------------------------------------- /Lecture 4/equality-constraints.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using ForwardDiff\n", 20 | "using PyPlot" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "Q = Diagonal([0.5; 1])\n", 30 | "function f(x)\n", 31 | " return 0.5*(x-[1; 0])'*Q*(x-[1; 0])\n", 32 | "end\n", 33 | "function ∇f(x)\n", 34 | " return Q*(x-[1; 0])\n", 35 | "end\n", 36 | "function ∇2f(x)\n", 37 | " return Q\n", 38 | "end" 39 | ] 40 | }, 41 | { 42 | "cell_type": "code", 43 | "execution_count": null, 44 | "metadata": {}, 45 | "outputs": [], 46 | "source": [ 47 | "function c(x)\n", 48 | " return x[1]^2 + 2*x[1] - x[2]\n", 49 | "end\n", 50 | "function ∂c(x)\n", 51 | " return [2*x[1]+2 -1]\n", 52 | "end" 53 | ] 54 | }, 55 | { 56 | "cell_type": "code", 57 | "execution_count": null, 58 | "metadata": {}, 59 | "outputs": [], 60 | "source": [ 61 | "function plot_landscape()\n", 62 | " Nsamp = 20\n", 63 | " Xsamp = kron(ones(Nsamp),LinRange(-4,4,Nsamp)')\n", 64 | " Ysamp = kron(ones(Nsamp)',LinRange(-4,4,Nsamp))\n", 65 | " Zsamp = zeros(Nsamp,Nsamp)\n", 66 | " for j = 1:Nsamp\n", 67 | " for k = 1:Nsamp\n", 68 | " Zsamp[j,k] = f([Xsamp[j,k]; Ysamp[j,k]])\n", 69 | " end\n", 70 | " end\n", 71 | " contour(Xsamp,Ysamp,Zsamp)\n", 72 | "\n", 73 | " xc = LinRange(-3.2,1.2,Nsamp)\n", 74 | " plot(xc,xc.^2+2.0.*xc,\"y\")\n", 75 | "end\n", 76 | "\n", 77 | "plot_landscape()" 78 | ] 79 | }, 80 | { 81 | "cell_type": "code", 82 | "execution_count": null, 83 | "metadata": {}, 84 | "outputs": [], 85 | "source": [ 86 | "function newton_step(x0,λ0)\n", 87 | " H = ∇2f(x0) + ForwardDiff.jacobian(x -> ∂c(x)'*λ0, x0)\n", 88 | " C = ∂c(x0)\n", 89 | " Δz = [H C'; C 0]\\[-∇f(x0)-C'*λ0; -c(x0)]\n", 90 | " Δx = Δz[1:2]\n", 91 | " Δλ = Δz[3]\n", 92 | " return x0+Δx, λ0+Δλ\n", 93 | "end" 94 | ] 95 | }, 96 | { 97 | "cell_type": "code", 98 | "execution_count": null, 99 | "metadata": {}, 100 | "outputs": [], 101 | "source": [ 102 | "xguess = [-1; -1]\n", 103 | "λguess = [0.0]\n", 104 | "plot_landscape()\n", 105 | "plot(xguess[1], xguess[2], \"rx\")" 106 | ] 107 | }, 108 | { 109 | "cell_type": "code", 110 | "execution_count": null, 111 | "metadata": {}, 112 | "outputs": [], 113 | "source": [ 114 | "xnew, λnew = newton_step(xguess[:,end],λguess[end])\n", 115 | "xguess = [xguess xnew]\n", 116 | "λguess = [λguess λnew]\n", 117 | "plot_landscape()\n", 118 | "plot(xguess[1,:], xguess[2,:],\n", 119 | " \"rx\")" 120 | ] 121 | }, 122 | { 123 | "cell_type": "code", 124 | "execution_count": null, 125 | "metadata": {}, 126 | "outputs": [], 127 | "source": [ 128 | "H = ∇2f(xguess[:,end]) + ForwardDiff.jacobian(x -> ∂c(x)'*λguess[end], xguess[:,end])" 129 | ] 130 | }, 131 | { 132 | "cell_type": "code", 133 | "execution_count": null, 134 | "metadata": {}, 135 | "outputs": [], 136 | "source": [] 137 | }, 138 | { 139 | "cell_type": "code", 140 | "execution_count": null, 141 | "metadata": {}, 142 | "outputs": [], 143 | "source": [ 144 | "function gauss_newton_step(x0,λ0)\n", 145 | " H = ∇2f(x0)\n", 146 | " C = ∂c(x0)\n", 147 | " Δz = [H C'; C 0]\\[-∇f(x0)-C'*λ0; -c(x0)]\n", 148 | " Δx = Δz[1:2]\n", 149 | " Δλ = Δz[3]\n", 150 | " return x0+Δx, λ0+Δλ\n", 151 | "end" 152 | ] 153 | }, 154 | { 155 | "cell_type": "code", 156 | "execution_count": null, 157 | "metadata": {}, 158 | "outputs": [], 159 | "source": [ 160 | "xguess = [-3; 2]\n", 161 | "λguess = [0.0]\n", 162 | "plot_landscape()\n", 163 | "plot(xguess[1], xguess[2], \"rx\")" 164 | ] 165 | }, 166 | { 167 | "cell_type": "code", 168 | "execution_count": null, 169 | "metadata": {}, 170 | "outputs": [], 171 | "source": [ 172 | "xnew, λnew = gauss_newton_step(xguess[:,end],λguess[end])\n", 173 | "xguess = [xguess xnew]\n", 174 | "λguess = [λguess λnew]\n", 175 | "plot_landscape()\n", 176 | "plot(xguess[1,:], xguess[2,:], \"rx\")" 177 | ] 178 | }, 179 | { 180 | "cell_type": "code", 181 | "execution_count": null, 182 | "metadata": {}, 183 | "outputs": [], 184 | "source": [] 185 | }, 186 | { 187 | "cell_type": "code", 188 | "execution_count": null, 189 | "metadata": {}, 190 | "outputs": [], 191 | "source": [] 192 | } 193 | ], 194 | "metadata": { 195 | "kernelspec": { 196 | "display_name": "Julia 1.6.5", 197 | "language": "julia", 198 | "name": "julia-1.6" 199 | }, 200 | "language_info": { 201 | "file_extension": ".jl", 202 | "mimetype": "application/julia", 203 | "name": "julia", 204 | "version": "1.6.5" 205 | } 206 | }, 207 | "nbformat": 4, 208 | "nbformat_minor": 4 209 | } 210 | -------------------------------------------------------------------------------- /Lecture 4/minimization.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using ForwardDiff\n", 20 | "using PyPlot" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "function f(x)\n", 30 | " return x.^4 + x.^3 - x.^2 - x\n", 31 | "end" 32 | ] 33 | }, 34 | { 35 | "cell_type": "code", 36 | "execution_count": null, 37 | "metadata": {}, 38 | "outputs": [], 39 | "source": [ 40 | "function ∇f(x)\n", 41 | " return 4.0*x.^3 + 3.0*x.^2 - 2.0*x - 1.0\n", 42 | "end" 43 | ] 44 | }, 45 | { 46 | "cell_type": "code", 47 | "execution_count": null, 48 | "metadata": {}, 49 | "outputs": [], 50 | "source": [ 51 | "function ∇2f(x)\n", 52 | " return 12.0*x.^2 + 6.0*x - 2.0\n", 53 | "end" 54 | ] 55 | }, 56 | { 57 | "cell_type": "code", 58 | "execution_count": null, 59 | "metadata": {}, 60 | "outputs": [], 61 | "source": [ 62 | "x = LinRange(-1.75,1.25,1000)" 63 | ] 64 | }, 65 | { 66 | "cell_type": "code", 67 | "execution_count": null, 68 | "metadata": {}, 69 | "outputs": [], 70 | "source": [ 71 | "p = plot(x,f(x))" 72 | ] 73 | }, 74 | { 75 | "cell_type": "code", 76 | "execution_count": null, 77 | "metadata": {}, 78 | "outputs": [], 79 | "source": [ 80 | "function newton_step(x0)\n", 81 | " xn = x0 - ∇2f(x0)\\∇f(x0)\n", 82 | "end" 83 | ] 84 | }, 85 | { 86 | "cell_type": "code", 87 | "execution_count": null, 88 | "metadata": {}, 89 | "outputs": [], 90 | "source": [ 91 | "xguess = 0.0\n", 92 | "plot(x, f(x))\n", 93 | "plot(xguess, f(xguess), \"rx\")" 94 | ] 95 | }, 96 | { 97 | "cell_type": "code", 98 | "execution_count": null, 99 | "metadata": {}, 100 | "outputs": [], 101 | "source": [ 102 | "xnew = newton_step(xguess[end])\n", 103 | "xguess = [xguess xnew]\n", 104 | "plot(x, f(x))\n", 105 | "plot(xguess, f(xguess), \"rx\")" 106 | ] 107 | }, 108 | { 109 | "cell_type": "code", 110 | "execution_count": null, 111 | "metadata": {}, 112 | "outputs": [], 113 | "source": [] 114 | }, 115 | { 116 | "cell_type": "code", 117 | "execution_count": null, 118 | "metadata": {}, 119 | "outputs": [], 120 | "source": [ 121 | "∇2f(0.0)" 122 | ] 123 | }, 124 | { 125 | "cell_type": "code", 126 | "execution_count": null, 127 | "metadata": {}, 128 | "outputs": [], 129 | "source": [ 130 | "function regularized_newton_step(x0)\n", 131 | " β = 1.0\n", 132 | " H = ∇2f(x0)\n", 133 | " while !isposdef(H)\n", 134 | " H = H + β*I\n", 135 | " end\n", 136 | " xn = x0 - H\\∇f(x0)\n", 137 | "end" 138 | ] 139 | }, 140 | { 141 | "cell_type": "code", 142 | "execution_count": null, 143 | "metadata": {}, 144 | "outputs": [], 145 | "source": [ 146 | "xguess = 0.0\n", 147 | "plot(x, f(x))\n", 148 | "plot(xguess, f(xguess), \"rx\")" 149 | ] 150 | }, 151 | { 152 | "cell_type": "code", 153 | "execution_count": null, 154 | "metadata": {}, 155 | "outputs": [], 156 | "source": [ 157 | "xnew = regularized_newton_step(xguess[end])\n", 158 | "xguess = [xguess xnew]\n", 159 | "plot(x, f(x))\n", 160 | "plot(xguess, f(xguess), \"rx\")" 161 | ] 162 | }, 163 | { 164 | "cell_type": "code", 165 | "execution_count": null, 166 | "metadata": {}, 167 | "outputs": [], 168 | "source": [] 169 | }, 170 | { 171 | "cell_type": "code", 172 | "execution_count": null, 173 | "metadata": {}, 174 | "outputs": [], 175 | "source": [ 176 | "function backtracking_regularized_newton_step(x0)\n", 177 | " b = 0.1\n", 178 | " c = 0.5\n", 179 | " β = 1.0\n", 180 | " H = ∇2f(x0)\n", 181 | " while !isposdef(H)\n", 182 | " H = H + β*I\n", 183 | " end\n", 184 | " Δx = -H\\∇f(x0)\n", 185 | " \n", 186 | " α = 1.0\n", 187 | " while f(x0 + α*Δx) > f(x0) + b*α*∇f(x0)\n", 188 | " α = c*α\n", 189 | " end\n", 190 | " \n", 191 | " xn = x0 + α*Δx\n", 192 | "end" 193 | ] 194 | }, 195 | { 196 | "cell_type": "code", 197 | "execution_count": null, 198 | "metadata": {}, 199 | "outputs": [], 200 | "source": [ 201 | "xguess = 0.0\n", 202 | "plot(x, f(x))\n", 203 | "plot(xguess, f(xguess), \"rx\")" 204 | ] 205 | }, 206 | { 207 | "cell_type": "code", 208 | "execution_count": null, 209 | "metadata": {}, 210 | "outputs": [], 211 | "source": [ 212 | "xnew = backtracking_regularized_newton_step(xguess[end])\n", 213 | "xguess = [xguess xnew]\n", 214 | "plot(x, f(x))\n", 215 | "plot(xguess, f(xguess), \"rx\")" 216 | ] 217 | }, 218 | { 219 | "cell_type": "code", 220 | "execution_count": null, 221 | "metadata": {}, 222 | "outputs": [], 223 | "source": [] 224 | } 225 | ], 226 | "metadata": { 227 | "kernelspec": { 228 | "display_name": "Julia 1.6.5", 229 | "language": "julia", 230 | "name": "julia-1.6" 231 | }, 232 | "language_info": { 233 | "file_extension": ".jl", 234 | "mimetype": "application/julia", 235 | "name": "julia", 236 | "version": "1.6.5" 237 | } 238 | }, 239 | "nbformat": 4, 240 | "nbformat_minor": 4 241 | } 242 | -------------------------------------------------------------------------------- /Lecture 5/Lecture 5.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 5/Lecture 5.pdf -------------------------------------------------------------------------------- /Lecture 5/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | [[ArgTools]] 4 | uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f" 5 | 6 | [[Artifacts]] 7 | uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33" 8 | 9 | [[Base64]] 10 | uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" 11 | 12 | [[ChainRulesCore]] 13 | deps = ["Compat", "LinearAlgebra", "SparseArrays"] 14 | git-tree-sha1 = "54fc4400de6e5c3e27be6047da2ef6ba355511f8" 15 | uuid = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" 16 | version = "1.11.6" 17 | 18 | [[ChangesOfVariables]] 19 | deps = ["ChainRulesCore", "LinearAlgebra", "Test"] 20 | git-tree-sha1 = "bf98fa45a0a4cee295de98d4c1462be26345b9a1" 21 | uuid = "9e997f8a-9a97-42d5-a9f1-ce6bfc15e2c0" 22 | version = "0.1.2" 23 | 24 | [[CommonSubexpressions]] 25 | deps = ["MacroTools", "Test"] 26 | git-tree-sha1 = "7b8a93dba8af7e3b42fecabf646260105ac373f7" 27 | uuid = "bbf7d656-a473-5ed7-a52c-81e309532950" 28 | version = "0.3.0" 29 | 30 | [[Compat]] 31 | deps = ["Base64", "Dates", "DelimitedFiles", "Distributed", "InteractiveUtils", "LibGit2", "Libdl", "LinearAlgebra", "Markdown", "Mmap", "Pkg", "Printf", "REPL", "Random", "SHA", "Serialization", "SharedArrays", "Sockets", "SparseArrays", "Statistics", "Test", "UUIDs", "Unicode"] 32 | git-tree-sha1 = "44c37b4636bc54afac5c574d2d02b625349d6582" 33 | uuid = "34da2185-b29b-5c13-b0c7-acf172513d20" 34 | version = "3.41.0" 35 | 36 | [[CompilerSupportLibraries_jll]] 37 | deps = ["Artifacts", "Libdl"] 38 | uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" 39 | 40 | [[Dates]] 41 | deps = ["Printf"] 42 | uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" 43 | 44 | [[DelimitedFiles]] 45 | deps = ["Mmap"] 46 | uuid = "8bb1440f-4735-579b-a4ab-409b98df4dab" 47 | 48 | [[DiffResults]] 49 | deps = ["StaticArrays"] 50 | git-tree-sha1 = "c18e98cba888c6c25d1c3b048e4b3380ca956805" 51 | uuid = "163ba53b-c6d8-5494-b064-1a9d43ac40c5" 52 | version = "1.0.3" 53 | 54 | [[DiffRules]] 55 | deps = ["LogExpFunctions", "NaNMath", "Random", "SpecialFunctions"] 56 | git-tree-sha1 = "9bc5dac3c8b6706b58ad5ce24cffd9861f07c94f" 57 | uuid = "b552c78f-8df3-52c6-915a-8e097449b14b" 58 | version = "1.9.0" 59 | 60 | [[Distributed]] 61 | deps = ["Random", "Serialization", "Sockets"] 62 | uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b" 63 | 64 | [[DocStringExtensions]] 65 | deps = ["LibGit2"] 66 | git-tree-sha1 = "b19534d1895d702889b219c382a6e18010797f0b" 67 | uuid = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" 68 | version = "0.8.6" 69 | 70 | [[Downloads]] 71 | deps = ["ArgTools", "LibCURL", "NetworkOptions"] 72 | uuid = "f43a241f-c20a-4ad4-852c-f6b1247861c6" 73 | 74 | [[ForwardDiff]] 75 | deps = ["CommonSubexpressions", "DiffResults", "DiffRules", "LinearAlgebra", "LogExpFunctions", "NaNMath", "Preferences", "Printf", "Random", "SpecialFunctions", "StaticArrays"] 76 | git-tree-sha1 = "1bd6fc0c344fc0cbee1f42f8d2e7ec8253dda2d2" 77 | uuid = "f6369f11-7733-5829-9624-2563aa707210" 78 | version = "0.10.25" 79 | 80 | [[InteractiveUtils]] 81 | deps = ["Markdown"] 82 | uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" 83 | 84 | [[InverseFunctions]] 85 | deps = ["Test"] 86 | git-tree-sha1 = "a7254c0acd8e62f1ac75ad24d5db43f5f19f3c65" 87 | uuid = "3587e190-3f89-42d0-90ee-14403ec27112" 88 | version = "0.1.2" 89 | 90 | [[IrrationalConstants]] 91 | git-tree-sha1 = "7fd44fd4ff43fc60815f8e764c0f352b83c49151" 92 | uuid = "92d709cd-6900-40b7-9082-c6be49f344b6" 93 | version = "0.1.1" 94 | 95 | [[JLLWrappers]] 96 | deps = ["Preferences"] 97 | git-tree-sha1 = "22df5b96feef82434b07327e2d3c770a9b21e023" 98 | uuid = "692b3bcd-3c85-4b1f-b108-f13ce0eb3210" 99 | version = "1.4.0" 100 | 101 | [[LibCURL]] 102 | deps = ["LibCURL_jll", "MozillaCACerts_jll"] 103 | uuid = "b27032c2-a3e7-50c8-80cd-2d36dbcbfd21" 104 | 105 | [[LibCURL_jll]] 106 | deps = ["Artifacts", "LibSSH2_jll", "Libdl", "MbedTLS_jll", "Zlib_jll", "nghttp2_jll"] 107 | uuid = "deac9b47-8bc7-5906-a0fe-35ac56dc84c0" 108 | 109 | [[LibGit2]] 110 | deps = ["Base64", "NetworkOptions", "Printf", "SHA"] 111 | uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" 112 | 113 | [[LibSSH2_jll]] 114 | deps = ["Artifacts", "Libdl", "MbedTLS_jll"] 115 | uuid = "29816b5a-b9ab-546f-933c-edad1886dfa8" 116 | 117 | [[Libdl]] 118 | uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" 119 | 120 | [[LinearAlgebra]] 121 | deps = ["Libdl"] 122 | uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 123 | 124 | [[LogExpFunctions]] 125 | deps = ["ChainRulesCore", "ChangesOfVariables", "DocStringExtensions", "InverseFunctions", "IrrationalConstants", "LinearAlgebra"] 126 | git-tree-sha1 = "e5718a00af0ab9756305a0392832c8952c7426c1" 127 | uuid = "2ab3a3ac-af41-5b50-aa03-7779005ae688" 128 | version = "0.3.6" 129 | 130 | [[Logging]] 131 | uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" 132 | 133 | [[MacroTools]] 134 | deps = ["Markdown", "Random"] 135 | git-tree-sha1 = "3d3e902b31198a27340d0bf00d6ac452866021cf" 136 | uuid = "1914dd2f-81c6-5fcd-8719-6d5c9610ff09" 137 | version = "0.5.9" 138 | 139 | [[Markdown]] 140 | deps = ["Base64"] 141 | uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" 142 | 143 | [[MbedTLS_jll]] 144 | deps = ["Artifacts", "Libdl"] 145 | uuid = "c8ffd9c3-330d-5841-b78e-0817d7145fa1" 146 | 147 | [[Mmap]] 148 | uuid = "a63ad114-7e13-5084-954f-fe012c677804" 149 | 150 | [[MozillaCACerts_jll]] 151 | uuid = "14a3606d-f60d-562e-9121-12d972cd8159" 152 | 153 | [[NaNMath]] 154 | git-tree-sha1 = "f755f36b19a5116bb580de457cda0c140153f283" 155 | uuid = "77ba4419-2d1f-58cd-9bb1-8ffee604a2e3" 156 | version = "0.3.6" 157 | 158 | [[NetworkOptions]] 159 | uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908" 160 | 161 | [[OpenLibm_jll]] 162 | deps = ["Artifacts", "Libdl"] 163 | uuid = "05823500-19ac-5b8b-9628-191a04bc5112" 164 | 165 | [[OpenSpecFun_jll]] 166 | deps = ["Artifacts", "CompilerSupportLibraries_jll", "JLLWrappers", "Libdl", "Pkg"] 167 | git-tree-sha1 = "13652491f6856acfd2db29360e1bbcd4565d04f1" 168 | uuid = "efe28fd5-8261-553b-a9e1-b2916fc3738e" 169 | version = "0.5.5+0" 170 | 171 | [[Pkg]] 172 | deps = ["Artifacts", "Dates", "Downloads", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "Serialization", "TOML", "Tar", "UUIDs", "p7zip_jll"] 173 | uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" 174 | 175 | [[Preferences]] 176 | deps = ["TOML"] 177 | git-tree-sha1 = "2cf929d64681236a2e074ffafb8d568733d2e6af" 178 | uuid = "21216c6a-2e73-6563-6e65-726566657250" 179 | version = "1.2.3" 180 | 181 | [[Printf]] 182 | deps = ["Unicode"] 183 | uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" 184 | 185 | [[REPL]] 186 | deps = ["InteractiveUtils", "Markdown", "Sockets", "Unicode"] 187 | uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" 188 | 189 | [[Random]] 190 | deps = ["Serialization"] 191 | uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" 192 | 193 | [[SHA]] 194 | uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" 195 | 196 | [[Serialization]] 197 | uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" 198 | 199 | [[SharedArrays]] 200 | deps = ["Distributed", "Mmap", "Random", "Serialization"] 201 | uuid = "1a1011a3-84de-559e-8e89-a11a2f7dc383" 202 | 203 | [[Sockets]] 204 | uuid = "6462fe0b-24de-5631-8697-dd941f90decc" 205 | 206 | [[SparseArrays]] 207 | deps = ["LinearAlgebra", "Random"] 208 | uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" 209 | 210 | [[SpecialFunctions]] 211 | deps = ["ChainRulesCore", "IrrationalConstants", "LogExpFunctions", "OpenLibm_jll", "OpenSpecFun_jll"] 212 | git-tree-sha1 = "e6bf188613555c78062842777b116905a9f9dd49" 213 | uuid = "276daf66-3868-5448-9aa4-cd146d93841b" 214 | version = "2.1.0" 215 | 216 | [[StaticArrays]] 217 | deps = ["LinearAlgebra", "Random", "Statistics"] 218 | git-tree-sha1 = "2884859916598f974858ff01df7dfc6c708dd895" 219 | uuid = "90137ffa-7385-5640-81b9-e52037218182" 220 | version = "1.3.3" 221 | 222 | [[Statistics]] 223 | deps = ["LinearAlgebra", "SparseArrays"] 224 | uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" 225 | 226 | [[TOML]] 227 | deps = ["Dates"] 228 | uuid = "fa267f1f-6049-4f14-aa54-33bafae1ed76" 229 | 230 | [[Tar]] 231 | deps = ["ArgTools", "SHA"] 232 | uuid = "a4e569a6-e804-4fa4-b0f3-eef7a1d5b13e" 233 | 234 | [[Test]] 235 | deps = ["InteractiveUtils", "Logging", "Random", "Serialization"] 236 | uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40" 237 | 238 | [[UUIDs]] 239 | deps = ["Random", "SHA"] 240 | uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" 241 | 242 | [[Unicode]] 243 | uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" 244 | 245 | [[Zlib_jll]] 246 | deps = ["Libdl"] 247 | uuid = "83775a58-1f1d-513f-b197-d71354ab007a" 248 | 249 | [[nghttp2_jll]] 250 | deps = ["Artifacts", "Libdl"] 251 | uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d" 252 | 253 | [[p7zip_jll]] 254 | deps = ["Artifacts", "Libdl"] 255 | uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0" 256 | -------------------------------------------------------------------------------- /Lecture 5/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" 3 | -------------------------------------------------------------------------------- /Lecture 6/Control History.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 6/Control History.pdf -------------------------------------------------------------------------------- /Lecture 6/Lecture 6.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 6/Lecture 6.pdf -------------------------------------------------------------------------------- /Lecture 7/Lecture 7.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 7/Lecture 7.pdf -------------------------------------------------------------------------------- /Lecture 7/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e" 3 | -------------------------------------------------------------------------------- /Lecture 7/lqr-qp.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 14, 6 | "metadata": {}, 7 | "outputs": [ 8 | { 9 | "name": "stderr", 10 | "output_type": "stream", 11 | "text": [ 12 | "\u001b[32m\u001b[1m Activating\u001b[22m\u001b[39m environment at `~/GitHub/lecture-notebooks/Lecture 7/Project.toml`\n" 13 | ] 14 | } 15 | ], 16 | "source": [ 17 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 18 | ] 19 | }, 20 | { 21 | "cell_type": "code", 22 | "execution_count": 15, 23 | "metadata": {}, 24 | "outputs": [], 25 | "source": [ 26 | "using LinearAlgebra\n", 27 | "using PyPlot\n", 28 | "using SparseArrays" 29 | ] 30 | }, 31 | { 32 | "cell_type": "code", 33 | "execution_count": 16, 34 | "metadata": {}, 35 | "outputs": [ 36 | { 37 | "data": { 38 | "text/plain": [ 39 | "2-element Vector{Float64}:\n", 40 | " 0.005000000000000001\n", 41 | " 0.1" 42 | ] 43 | }, 44 | "execution_count": 16, 45 | "metadata": {}, 46 | "output_type": "execute_result" 47 | } 48 | ], 49 | "source": [ 50 | "# Discrete dynamics\n", 51 | "h = 0.1 # time step\n", 52 | "A = [1 h; 0 1]\n", 53 | "B = [0.5*h*h; h]" 54 | ] 55 | }, 56 | { 57 | "cell_type": "code", 58 | "execution_count": 17, 59 | "metadata": {}, 60 | "outputs": [], 61 | "source": [ 62 | "n = 2 # number of state\n", 63 | "m = 1 # number of controls\n", 64 | "Tfinal = 10.0 # final time\n", 65 | "N = Int(Tfinal/h)+1 # number of time steps\n", 66 | "thist = Array(range(0,h*(N-1), step=h));" 67 | ] 68 | }, 69 | { 70 | "cell_type": "code", 71 | "execution_count": 18, 72 | "metadata": {}, 73 | "outputs": [ 74 | { 75 | "data": { 76 | "text/plain": [ 77 | "2-element Vector{Float64}:\n", 78 | " 1.0\n", 79 | " 0.0" 80 | ] 81 | }, 82 | "execution_count": 18, 83 | "metadata": {}, 84 | "output_type": "execute_result" 85 | } 86 | ], 87 | "source": [ 88 | "# Initial conditions\n", 89 | "x0 = [1.0; 0]" 90 | ] 91 | }, 92 | { 93 | "cell_type": "code", 94 | "execution_count": 19, 95 | "metadata": {}, 96 | "outputs": [ 97 | { 98 | "data": { 99 | "text/plain": [ 100 | "2×2 SparseMatrixCSC{Float64, Int64} with 2 stored entries:\n", 101 | " 1.0 ⋅ \n", 102 | " ⋅ 1.0" 103 | ] 104 | }, 105 | "execution_count": 19, 106 | "metadata": {}, 107 | "output_type": "execute_result" 108 | } 109 | ], 110 | "source": [ 111 | "# Cost weights\n", 112 | "Q = sparse(1.0*I(2))\n", 113 | "R = sparse(0.1*I(1))\n", 114 | "Qn = sparse(1.0*I(2))" 115 | ] 116 | }, 117 | { 118 | "cell_type": "code", 119 | "execution_count": 20, 120 | "metadata": {}, 121 | "outputs": [ 122 | { 123 | "data": { 124 | "text/plain": [ 125 | "J (generic function with 1 method)" 126 | ] 127 | }, 128 | "execution_count": 20, 129 | "metadata": {}, 130 | "output_type": "execute_result" 131 | } 132 | ], 133 | "source": [ 134 | "#Cost function\n", 135 | "function J(xhist,uhist)\n", 136 | " cost = 0.5*xhist[:,end]'*Qn*xhist[:,end]\n", 137 | " for k = 1:(N-1)\n", 138 | " cost = cost + 0.5*xhist[:,k]'*Q*xhist[:,k] + 0.5*(uhist[k]'*R*uhist[k])[1]\n", 139 | " end\n", 140 | " return cost\n", 141 | "end" 142 | ] 143 | }, 144 | { 145 | "cell_type": "code", 146 | "execution_count": 21, 147 | "metadata": {}, 148 | "outputs": [], 149 | "source": [ 150 | "# Cost\n", 151 | "H = blockdiag(R, kron(I(N-2), blockdiag(Q,R)), Qn);" 152 | ] 153 | }, 154 | { 155 | "cell_type": "code", 156 | "execution_count": 22, 157 | "metadata": {}, 158 | "outputs": [], 159 | "source": [ 160 | "# Constraints\n", 161 | "C = kron(I(N-1), [B -I(2)])\n", 162 | "for k = 1:N-2\n", 163 | " C[(k*n).+(1:n), (k*(n+m)-n).+(1:n)] .= A\n", 164 | "end\n", 165 | "d = [-A*x0; zeros(size(C,1)-n)];" 166 | ] 167 | }, 168 | { 169 | "cell_type": "code", 170 | "execution_count": 23, 171 | "metadata": {}, 172 | "outputs": [], 173 | "source": [ 174 | "# Solve the linear system\n", 175 | "y = [H C'; C zeros(size(C,1),size(C,1))]\\[zeros(size(H,1)); d]\n", 176 | "\n", 177 | "# Get state history\n", 178 | "z = y[1:size(H,1)] # states and controls [u0,x1,u1,...,xN]\n", 179 | "Z = reshape(z,n+m,N-1)\n", 180 | "xhist = Z[m+1:end,:]\n", 181 | "xhist = [x0 xhist]\n", 182 | "\n", 183 | "# Get control history\n", 184 | "uhist = Z[1,:];" 185 | ] 186 | }, 187 | { 188 | "cell_type": "code", 189 | "execution_count": 24, 190 | "metadata": {}, 191 | "outputs": [ 192 | { 193 | "data": { 194 | "image/png": "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", 195 | "text/plain": [ 196 | "Figure(PyObject
)" 197 | ] 198 | }, 199 | "metadata": {}, 200 | "output_type": "display_data" 201 | }, 202 | { 203 | "data": { 204 | "text/plain": [ 205 | "PyObject " 206 | ] 207 | }, 208 | "execution_count": 24, 209 | "metadata": {}, 210 | "output_type": "execute_result" 211 | } 212 | ], 213 | "source": [ 214 | "# Plot x1 vs. x2, u vs. t, x vs. t, etc.\n", 215 | "times = range(0,h*(N-1), step=h)\n", 216 | "plot(times,xhist[1,:], label=\"position\")\n", 217 | "plot(times,xhist[2,:], label=\"velocity\")\n", 218 | "xlabel(\"time\")\n", 219 | "legend()" 220 | ] 221 | }, 222 | { 223 | "cell_type": "code", 224 | "execution_count": 25, 225 | "metadata": {}, 226 | "outputs": [ 227 | { 228 | "data": { 229 | "image/png": "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", 230 | "text/plain": [ 231 | "Figure(PyObject
)" 232 | ] 233 | }, 234 | "metadata": {}, 235 | "output_type": "display_data" 236 | }, 237 | { 238 | "data": { 239 | "text/plain": [ 240 | "PyObject " 241 | ] 242 | }, 243 | "execution_count": 25, 244 | "metadata": {}, 245 | "output_type": "execute_result" 246 | } 247 | ], 248 | "source": [ 249 | "plot(times[1:end-1], uhist, label=\"control\")\n", 250 | "xlabel(\"Time\")\n", 251 | "legend()" 252 | ] 253 | }, 254 | { 255 | "cell_type": "code", 256 | "execution_count": 26, 257 | "metadata": {}, 258 | "outputs": [ 259 | { 260 | "data": { 261 | "text/plain": [ 262 | "6.658612208961312" 263 | ] 264 | }, 265 | "execution_count": 26, 266 | "metadata": {}, 267 | "output_type": "execute_result" 268 | } 269 | ], 270 | "source": [ 271 | "J(xhist,uhist)" 272 | ] 273 | }, 274 | { 275 | "cell_type": "code", 276 | "execution_count": null, 277 | "metadata": {}, 278 | "outputs": [], 279 | "source": [] 280 | } 281 | ], 282 | "metadata": { 283 | "kernelspec": { 284 | "display_name": "Julia 1.6.5", 285 | "language": "julia", 286 | "name": "julia-1.6" 287 | }, 288 | "language_info": { 289 | "file_extension": ".jl", 290 | "mimetype": "application/julia", 291 | "name": "julia", 292 | "version": "1.6.5" 293 | } 294 | }, 295 | "nbformat": 4, 296 | "nbformat_minor": 4 297 | } 298 | -------------------------------------------------------------------------------- /Lecture 8/Lecture 8.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 8/Lecture 8.pdf -------------------------------------------------------------------------------- /Lecture 8/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e" 3 | ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" 4 | -------------------------------------------------------------------------------- /Lecture 9/Lecture 9.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 9/Lecture 9.pdf -------------------------------------------------------------------------------- /Lecture 9/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 9/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/Lecture 9/Project.toml -------------------------------------------------------------------------------- /Lecture 9/mpc.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using PyPlot\n", 20 | "using SparseArrays\n", 21 | "using ForwardDiff\n", 22 | "using ControlSystems\n", 23 | "using OSQP" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": null, 29 | "metadata": {}, 30 | "outputs": [], 31 | "source": [ 32 | "#Model parameters\n", 33 | "g = 9.81 #m/s^2\n", 34 | "m = 1.0 #kg \n", 35 | "ℓ = 0.3 #meters\n", 36 | "J = 0.2*m*ℓ*ℓ\n", 37 | "\n", 38 | "#Thrust limits\n", 39 | "umin = [0.2*m*g; 0.2*m*g]\n", 40 | "umax = [0.6*m*g; 0.6*m*g]\n", 41 | "\n", 42 | "h = 0.05 #time step (20 Hz)" 43 | ] 44 | }, 45 | { 46 | "cell_type": "code", 47 | "execution_count": null, 48 | "metadata": {}, 49 | "outputs": [], 50 | "source": [ 51 | "#Planar Quadrotor Dynamics\n", 52 | "function quad_dynamics(x,u)\n", 53 | " θ = x[3]\n", 54 | " \n", 55 | " ẍ = (1/m)*(u[1] + u[2])*sin(θ)\n", 56 | " ÿ = (1/m)*(u[1] + u[2])*cos(θ) - g\n", 57 | " θ̈ = (1/J)*(ℓ/2)*(u[2] - u[1])\n", 58 | " \n", 59 | " return [x[4:6]; ẍ; ÿ; θ̈]\n", 60 | "end" 61 | ] 62 | }, 63 | { 64 | "cell_type": "code", 65 | "execution_count": null, 66 | "metadata": {}, 67 | "outputs": [], 68 | "source": [ 69 | "function quad_dynamics_rk4(x,u)\n", 70 | " #RK4 integration with zero-order hold on u\n", 71 | " f1 = quad_dynamics(x, u)\n", 72 | " f2 = quad_dynamics(x + 0.5*h*f1, u)\n", 73 | " f3 = quad_dynamics(x + 0.5*h*f2, u)\n", 74 | " f4 = quad_dynamics(x + h*f3, u)\n", 75 | " return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 76 | "end" 77 | ] 78 | }, 79 | { 80 | "cell_type": "code", 81 | "execution_count": null, 82 | "metadata": {}, 83 | "outputs": [], 84 | "source": [ 85 | "#Linearized dynamics for hovering\n", 86 | "x_hover = zeros(6)\n", 87 | "u_hover = [0.5*m*g; 0.5*m*g]\n", 88 | "A = ForwardDiff.jacobian(x->quad_dynamics_rk4(x,u_hover),x_hover);\n", 89 | "B = ForwardDiff.jacobian(u->quad_dynamics_rk4(x_hover,u),u_hover);\n", 90 | "quad_dynamics_rk4(x_hover, u_hover)" 91 | ] 92 | }, 93 | { 94 | "cell_type": "code", 95 | "execution_count": null, 96 | "metadata": {}, 97 | "outputs": [], 98 | "source": [ 99 | "Nx = 6 # number of state\n", 100 | "Nu = 2 # number of controls\n", 101 | "Tfinal = 10.0 # final time\n", 102 | "Nt = Int(Tfinal/h)+1 # number of time steps\n", 103 | "thist = Array(range(0,h*(Nt-1), step=h));" 104 | ] 105 | }, 106 | { 107 | "cell_type": "code", 108 | "execution_count": null, 109 | "metadata": {}, 110 | "outputs": [], 111 | "source": [ 112 | "# Cost weights\n", 113 | "Q = Array(1.0*I(Nx));\n", 114 | "R = Array(.01*I(Nu));\n", 115 | "Qn = Array(1.0*I(Nx));" 116 | ] 117 | }, 118 | { 119 | "cell_type": "code", 120 | "execution_count": null, 121 | "metadata": {}, 122 | "outputs": [], 123 | "source": [ 124 | "#Cost function\n", 125 | "function cost(xhist,uhist)\n", 126 | " cost = 0.5*xhist[:,end]'*Qn*xhist[:,end]\n", 127 | " for k = 1:(size(xhist,2)-1)\n", 128 | " cost = cost + 0.5*xhist[:,k]'*Q*xhist[:,k] + 0.5*(uhist[k]'*R*uhist[k])[1]\n", 129 | " end\n", 130 | " return cost\n", 131 | "end" 132 | ] 133 | }, 134 | { 135 | "cell_type": "code", 136 | "execution_count": null, 137 | "metadata": {}, 138 | "outputs": [], 139 | "source": [ 140 | "#LQR Hover Controller\n", 141 | "P = dare(A,B,Q,R)\n", 142 | "K = dlqr(A,B,Q,R)\n", 143 | "\n", 144 | "function lqr_controller(t,x,K,xref)\n", 145 | " \n", 146 | " return u_hover - K*(x-xref)\n", 147 | "end" 148 | ] 149 | }, 150 | { 151 | "cell_type": "code", 152 | "execution_count": null, 153 | "metadata": {}, 154 | "outputs": [], 155 | "source": [ 156 | "#Build QP matrices for OSQP\n", 157 | "Nh = 20 #one second horizon at 20Hz\n", 158 | "Nx = 6\n", 159 | "Nu = 2\n", 160 | "U = kron(Diagonal(I,Nh), [I zeros(Nu,Nx)]) #Matrix that picks out all u\n", 161 | "Θ = kron(Diagonal(I,Nh), [0 0 0 0 1 0 0 0]) #Matrix that picks out all x3 (θ)\n", 162 | "H = sparse([kron(Diagonal(I,Nh-1),[R zeros(Nu,Nx); zeros(Nx,Nu) Q]) zeros((Nx+Nu)*(Nh-1), Nx+Nu); zeros(Nx+Nu,(Nx+Nu)*(Nh-1)) [R zeros(Nu,Nx); zeros(Nx,Nu) P]])\n", 163 | "b = zeros(Nh*(Nx+Nu))\n", 164 | "C = sparse([[B -I zeros(Nx,(Nh-1)*(Nu+Nx))]; zeros(Nx*(Nh-1),Nu) [kron(Diagonal(I,Nh-1), [A B]) zeros((Nh-1)*Nx,Nx)] + [zeros((Nh-1)*Nx,Nx) kron(Diagonal(I,Nh-1),[zeros(Nx,Nu) Diagonal(-I,Nx)])]])\n", 165 | "\n", 166 | "#Dynamics + Thrust limit constraints\n", 167 | "D = [C; U]\n", 168 | "lb = [zeros(Nx*Nh); kron(ones(Nh),umin-u_hover)]\n", 169 | "ub = [zeros(Nx*Nh); kron(ones(Nh),umax-u_hover)]\n", 170 | "\n", 171 | "#Dynamics + thrust limit + bound constraint on θ to keep the system within small-angle approximation\n", 172 | "#D = [C; U; Θ]\n", 173 | "#lb = [zeros(Nx*Nh); kron(ones(Nh),umin-u_hover); -0.2*ones(Nh)]\n", 174 | "#ub = [zeros(Nx*Nh); kron(ones(Nh),umax-u_hover); 0.2*ones(Nh)]\n", 175 | "\n", 176 | "prob = OSQP.Model()\n", 177 | "OSQP.setup!(prob; P=H, q=b, A=D, l=lb, u=ub, verbose=false, eps_abs=1e-8, eps_rel=1e-8, polish=1);" 178 | ] 179 | }, 180 | { 181 | "cell_type": "code", 182 | "execution_count": null, 183 | "metadata": {}, 184 | "outputs": [], 185 | "source": [ 186 | "#MPC Controller\n", 187 | "function mpc_controller(t,x,xref)\n", 188 | " \n", 189 | " #Update QP problem\n", 190 | " lb[1:6] .= -A*x\n", 191 | " ub[1:6] .= -A*x\n", 192 | " \n", 193 | " for j = 1:(Nh-1)\n", 194 | " b[(Nu+(j-1)*(Nx+Nu)).+(1:Nx)] .= -Q*xref\n", 195 | " end\n", 196 | " b[(Nu+(Nh-1)*(Nx+Nu)).+(1:Nx)] .= -P*xref\n", 197 | " \n", 198 | " OSQP.update!(prob, q=b, l=lb, u=ub)\n", 199 | "\n", 200 | " #Solve QP\n", 201 | " results = OSQP.solve!(prob)\n", 202 | " Δu = results.x[1:Nu]\n", 203 | "\n", 204 | " return u_hover + Δu\n", 205 | "end" 206 | ] 207 | }, 208 | { 209 | "cell_type": "code", 210 | "execution_count": null, 211 | "metadata": {}, 212 | "outputs": [], 213 | "source": [ 214 | "function closed_loop(x0,controller,N)\n", 215 | " xhist = zeros(length(x0),N)\n", 216 | " u0 = controller(1,x0)\n", 217 | " uhist = zeros(length(u0),N-1)\n", 218 | " uhist[:,1] .= u0\n", 219 | " xhist[:,1] .= x0\n", 220 | " for k = 1:(N-1)\n", 221 | " uk = controller(k,xhist[:,k])\n", 222 | " uhist[:,k] = max.(min.(umax, uk), umin) #enforce control limits\n", 223 | " xhist[:,k+1] .= quad_dynamics_rk4(xhist[:,k],uhist[:,k])\n", 224 | " end\n", 225 | " return xhist, uhist\n", 226 | "end" 227 | ] 228 | }, 229 | { 230 | "cell_type": "code", 231 | "execution_count": null, 232 | "metadata": {}, 233 | "outputs": [], 234 | "source": [ 235 | "x_ref = [0.0; 1.0; 0; 0; 0; 0]\n", 236 | "x0 = [1.0; 2.0; 0.0; 0; 0; 0]\n", 237 | "xhist1, uhist1 = closed_loop(x0, (t,x)->lqr_controller(t,x,K,x_ref), Nt);\n", 238 | "xhist2, uhist2 = closed_loop(x0, (t,x)->mpc_controller(t,x,x_ref), Nt);" 239 | ] 240 | }, 241 | { 242 | "cell_type": "code", 243 | "execution_count": null, 244 | "metadata": {}, 245 | "outputs": [], 246 | "source": [ 247 | "plot(thist,xhist1[1,:], label=\"x LQR\")\n", 248 | "plot(thist,xhist2[1,:], label=\"x MPC\")\n", 249 | "xlabel(\"time\")\n", 250 | "legend()" 251 | ] 252 | }, 253 | { 254 | "cell_type": "code", 255 | "execution_count": null, 256 | "metadata": {}, 257 | "outputs": [], 258 | "source": [ 259 | "plot(thist,xhist1[2,:], label=\"y LQR\")\n", 260 | "plot(thist,xhist2[2,:], label=\"y MPC\")\n", 261 | "xlabel(\"time\")\n", 262 | "legend()" 263 | ] 264 | }, 265 | { 266 | "cell_type": "code", 267 | "execution_count": null, 268 | "metadata": {}, 269 | "outputs": [], 270 | "source": [ 271 | "plot(thist,xhist1[3,:], label=\"θ LQR\")\n", 272 | "plot(thist,xhist2[3,:], label=\"θ MPC\")\n", 273 | "xlabel(\"time\")\n", 274 | "legend()" 275 | ] 276 | }, 277 | { 278 | "cell_type": "code", 279 | "execution_count": null, 280 | "metadata": {}, 281 | "outputs": [], 282 | "source": [ 283 | "plot(thist[1:end-1], uhist1[1,:], label=\"u1 LQR\")\n", 284 | "plot(thist[1:end-1], uhist2[1,:], label=\"u1 MPC\")\n", 285 | "xlabel(\"Time\")\n", 286 | "legend()" 287 | ] 288 | }, 289 | { 290 | "cell_type": "code", 291 | "execution_count": null, 292 | "metadata": {}, 293 | "outputs": [], 294 | "source": [ 295 | "plot(thist[1:end-1], uhist1[2,:], label=\"u2 LQR\")\n", 296 | "plot(thist[1:end-1], uhist2[2,:], label=\"u2 MPC\")\n", 297 | "xlabel(\"Time\")\n", 298 | "legend()" 299 | ] 300 | }, 301 | { 302 | "cell_type": "code", 303 | "execution_count": null, 304 | "metadata": {}, 305 | "outputs": [], 306 | "source": [ 307 | "#Set up visualization\n", 308 | "using MeshCat\n", 309 | "using RobotZoo: Quadrotor, PlanarQuadrotor\n", 310 | "using CoordinateTransformations, Rotations, Colors, StaticArrays, RobotDynamics\n", 311 | "\n", 312 | "function set_mesh!(vis, model::L;\n", 313 | " scaling=1.0, color=colorant\"black\"\n", 314 | " ) where {L <: Union{Quadrotor, PlanarQuadrotor}} \n", 315 | " # urdf_folder = joinpath(@__DIR__, \"..\", \"data\", \"meshes\")\n", 316 | " urdf_folder = @__DIR__\n", 317 | " # if scaling != 1.0\n", 318 | " # quad_scaling = 0.085 * scaling\n", 319 | " obj = joinpath(urdf_folder, \"quadrotor_scaled.obj\")\n", 320 | " if scaling != 1.0\n", 321 | " error(\"Scaling not implemented after switching to MeshCat 0.12\")\n", 322 | " end\n", 323 | " robot_obj = MeshFileGeometry(obj)\n", 324 | " mat = MeshPhongMaterial(color=color)\n", 325 | " setobject!(vis[\"robot\"][\"geom\"], robot_obj, mat)\n", 326 | " if hasfield(L, :ned)\n", 327 | " model.ned && settransform!(vis[\"robot\"][\"geom\"], LinearMap(RotX(pi)))\n", 328 | " end\n", 329 | "end\n", 330 | "\n", 331 | "function visualize!(vis, model::PlanarQuadrotor, x::StaticVector)\n", 332 | " py,pz = x[1], x[2]\n", 333 | " θ = x[3]\n", 334 | " settransform!(vis[\"robot\"], compose(Translation(0,py,pz), LinearMap(RotX(-θ))))\n", 335 | "end\n", 336 | "\n", 337 | "function visualize!(vis, model, tf::Real, X)\n", 338 | " fps = Int(round((length(X)-1)/tf))\n", 339 | " anim = MeshCat.Animation(fps)\n", 340 | " n = state_dim(model)\n", 341 | " for (k,x) in enumerate(X)\n", 342 | " atframe(anim, k) do\n", 343 | " x = X[k]\n", 344 | " visualize!(vis, model, SVector{n}(x)) \n", 345 | " end\n", 346 | " end\n", 347 | " setanimation!(vis, anim)\n", 348 | "end" 349 | ] 350 | }, 351 | { 352 | "cell_type": "code", 353 | "execution_count": null, 354 | "metadata": {}, 355 | "outputs": [], 356 | "source": [ 357 | "vis = Visualizer()\n", 358 | "model = PlanarQuadrotor()\n", 359 | "set_mesh!(vis, model)\n", 360 | "render(vis)" 361 | ] 362 | }, 363 | { 364 | "cell_type": "code", 365 | "execution_count": null, 366 | "metadata": {}, 367 | "outputs": [], 368 | "source": [ 369 | "X1 = [SVector{6}(x) for x in eachcol(xhist1)];\n", 370 | "X2 = [SVector{6}(x) for x in eachcol(xhist2)];" 371 | ] 372 | }, 373 | { 374 | "cell_type": "code", 375 | "execution_count": null, 376 | "metadata": {}, 377 | "outputs": [], 378 | "source": [ 379 | "visualize!(vis, model, thist[end], X1)" 380 | ] 381 | }, 382 | { 383 | "cell_type": "code", 384 | "execution_count": null, 385 | "metadata": {}, 386 | "outputs": [], 387 | "source": [ 388 | "visualize!(vis, model, thist[end], X2)" 389 | ] 390 | }, 391 | { 392 | "cell_type": "code", 393 | "execution_count": null, 394 | "metadata": {}, 395 | "outputs": [], 396 | "source": [] 397 | } 398 | ], 399 | "metadata": { 400 | "kernelspec": { 401 | "display_name": "Julia 1.6.5", 402 | "language": "julia", 403 | "name": "julia-1.6" 404 | }, 405 | "language_info": { 406 | "file_extension": ".jl", 407 | "mimetype": "application/julia", 408 | "name": "julia", 409 | "version": "1.6.5" 410 | } 411 | }, 412 | "nbformat": 4, 413 | "nbformat_minor": 4 414 | } 415 | -------------------------------------------------------------------------------- /Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | julia_version = "1.6.5" 4 | manifest_format = "2.0" 5 | 6 | [[deps.Libdl]] 7 | uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" 8 | 9 | [[deps.LinearAlgebra]] 10 | deps = ["Libdl"] 11 | uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 12 | 13 | [[deps.Random]] 14 | deps = ["Serialization"] 15 | uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" 16 | 17 | [[deps.Serialization]] 18 | uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" 19 | 20 | [[deps.SparseArrays]] 21 | deps = ["LinearAlgebra", "Random"] 22 | uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" 23 | 24 | [[deps.StaticArrays]] 25 | deps = ["LinearAlgebra", "Random", "Statistics"] 26 | git-tree-sha1 = "6976fab022fea2ffea3d945159317556e5dad87c" 27 | uuid = "90137ffa-7385-5640-81b9-e52037218182" 28 | version = "1.4.2" 29 | 30 | [[deps.Statistics]] 31 | deps = ["LinearAlgebra", "SparseArrays"] 32 | uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" 33 | -------------------------------------------------------------------------------- /Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 16-745 Lecture Notebooks 2 | 3 | Hand-written lecture notes and corresponding Jupyter notebooks from the course Optimal Control and Reinforcement Learning as taught in the Robotics Institute at Carnegie Mellon University in spring 2022. 4 | 5 |

This work is licensed under CC BY-NC-SA 4.0

6 | -------------------------------------------------------------------------------- /misc/AL_tutorial.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2022/b92dc0b5a4692f1bc90d6a753bfdf175f03a0cef/misc/AL_tutorial.pdf --------------------------------------------------------------------------------