├── .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 └── guess.jld2 ├── 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 ├── Manifest.toml ├── Project.toml ├── policy-gradient-lqr-quadrotor.ipynb ├── policy-gradient.ipynb └── q-learning.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 ├── Untitled.ipynb ├── mpc.ipynb └── quadrotor_scaled.obj └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | *.DS_Store 3 | *.ipynb_checkpoints 4 | -------------------------------------------------------------------------------- /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-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 1/Intro.pdf -------------------------------------------------------------------------------- /Lecture 1/Lecture 1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 1/Lecture 1.pdf -------------------------------------------------------------------------------- /Lecture 10/Lecture 10.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 10/Lecture 10.pdf -------------------------------------------------------------------------------- /Lecture 10/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | MatrixCalculus = "fafa779c-62f8-11e9-0c41-4d0261472a5f" 3 | MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11" 4 | ProgressMeter = "92933f4c-e287-5a05-a399-4b506db050ca" 5 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 6 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 7 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 8 | TrajOptPlots = "7770976a-8dee-4930-bf39-a1782fd21ce6" 9 | -------------------------------------------------------------------------------- /Lecture 10/acrobot-ilqr.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 RobotZoo\n", 22 | "using RobotDynamics\n", 23 | "using MatrixCalculus" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": null, 29 | "metadata": {}, 30 | "outputs": [], 31 | "source": [ 32 | "#Acrobot Dynamics\n", 33 | "a = RobotZoo.Acrobot()\n", 34 | "h = 0.05" 35 | ] 36 | }, 37 | { 38 | "cell_type": "code", 39 | "execution_count": null, 40 | "metadata": {}, 41 | "outputs": [], 42 | "source": [ 43 | "function dynamics_rk4(x,u)\n", 44 | " #RK4 integration with zero-order hold on u\n", 45 | " f1 = RobotDynamics.dynamics(a, x, u)\n", 46 | " f2 = RobotDynamics.dynamics(a, x + 0.5*h*f1, u)\n", 47 | " f3 = RobotDynamics.dynamics(a, x + 0.5*h*f2, u)\n", 48 | " f4 = RobotDynamics.dynamics(a, x + h*f3, u)\n", 49 | " return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 50 | "end" 51 | ] 52 | }, 53 | { 54 | "cell_type": "code", 55 | "execution_count": null, 56 | "metadata": {}, 57 | "outputs": [], 58 | "source": [ 59 | "function dfdx(x,u)\n", 60 | " ForwardDiff.jacobian(dx->dynamics_rk4(dx,u),x)\n", 61 | "end\n", 62 | "\n", 63 | "function dfdu(x,u)\n", 64 | " ForwardDiff.derivative(du->dynamics_rk4(x,du),u)\n", 65 | "end" 66 | ] 67 | }, 68 | { 69 | "cell_type": "code", 70 | "execution_count": null, 71 | "metadata": {}, 72 | "outputs": [], 73 | "source": [ 74 | "Nx = 4 # number of state\n", 75 | "Nu = 1 # number of controls\n", 76 | "Tfinal = 10.0 # final time\n", 77 | "Nt = Int(Tfinal/h)+1 # number of time steps\n", 78 | "thist = Array(range(0,h*(Nt-1), step=h));" 79 | ] 80 | }, 81 | { 82 | "cell_type": "code", 83 | "execution_count": null, 84 | "metadata": {}, 85 | "outputs": [], 86 | "source": [ 87 | "# Cost weights\n", 88 | "Q = Diagonal([1.0*ones(2); 0.1*ones(2)]);\n", 89 | "R = 0.01;\n", 90 | "Qn = Array(100.0*I(Nx));" 91 | ] 92 | }, 93 | { 94 | "cell_type": "code", 95 | "execution_count": null, 96 | "metadata": {}, 97 | "outputs": [], 98 | "source": [ 99 | "function stage_cost(x,u)\n", 100 | " return 0.5*((x-xgoal)'*Q*(x-xgoal)) + 0.5*R*u*u\n", 101 | "end" 102 | ] 103 | }, 104 | { 105 | "cell_type": "code", 106 | "execution_count": null, 107 | "metadata": {}, 108 | "outputs": [], 109 | "source": [ 110 | "function terminal_cost(x)\n", 111 | " return 0.5*(x-xgoal)'*Qn*(x-xgoal)\n", 112 | "end" 113 | ] 114 | }, 115 | { 116 | "cell_type": "code", 117 | "execution_count": null, 118 | "metadata": {}, 119 | "outputs": [], 120 | "source": [ 121 | "function cost(xtraj,utraj)\n", 122 | " J = 0.0\n", 123 | " for k = 1:(Nt-1)\n", 124 | " J += stage_cost(xtraj[:,k],utraj[k])\n", 125 | " end\n", 126 | " J += terminal_cost(xtraj[:,Nt])\n", 127 | " return J\n", 128 | "end" 129 | ] 130 | }, 131 | { 132 | "cell_type": "code", 133 | "execution_count": null, 134 | "metadata": {}, 135 | "outputs": [], 136 | "source": [ 137 | "#Initial guess\n", 138 | "x0 = [-pi/2; 0; 0; 0]\n", 139 | "xgoal = [pi/2; 0; 0; 0]\n", 140 | "xtraj = kron(ones(1,Nt), x0)\n", 141 | "utraj = randn(Nt-1);" 142 | ] 143 | }, 144 | { 145 | "cell_type": "code", 146 | "execution_count": null, 147 | "metadata": {}, 148 | "outputs": [], 149 | "source": [ 150 | "#Initial Rollout\n", 151 | "for k = 1:(Nt-1)\n", 152 | " xtraj[:,k+1] .= dynamics_rk4(xtraj[:,k],utraj[k])\n", 153 | "end\n", 154 | "J = cost(xtraj,utraj)" 155 | ] 156 | }, 157 | { 158 | "cell_type": "code", 159 | "execution_count": null, 160 | "metadata": {}, 161 | "outputs": [], 162 | "source": [ 163 | "#DDP Algorithm\n", 164 | "p = zeros(Nx,Nt)\n", 165 | "P = zeros(Nx,Nx,Nt)\n", 166 | "d = ones(Nt-1)\n", 167 | "K = zeros(Nu,Nx,Nt-1)\n", 168 | "ΔJ = 0.0\n", 169 | "\n", 170 | "xn = zeros(Nx,Nt)\n", 171 | "un = zeros(Nt-1)\n", 172 | "\n", 173 | "gx = zeros(Nx)\n", 174 | "gu = 0.0\n", 175 | "Gxx = zeros(Nx,Nx)\n", 176 | "Guu = 0.0\n", 177 | "Gxu = zeros(Nx)\n", 178 | "Gux = zeros(Nx)\n", 179 | "\n", 180 | "iter = 0\n", 181 | "while maximum(abs.(d[:])) > 1e-3\n", 182 | " iter += 1\n", 183 | " \n", 184 | " p = zeros(Nx,Nt)\n", 185 | " P = zeros(Nx,Nx,Nt)\n", 186 | " d = ones(Nt-1)\n", 187 | " K = zeros(Nu,Nx,Nt-1)\n", 188 | " ΔJ = 0.0\n", 189 | "\n", 190 | " p[:,Nt] = Qn*(xtraj[:,Nt]-xgoal)\n", 191 | " P[:,:,Nt] = Qn\n", 192 | " \n", 193 | " #Backward Pass\n", 194 | " for k = (Nt-1):-1:1\n", 195 | " #Calculate derivatives\n", 196 | " q = Q*(xtraj[:,k]-xgoal)\n", 197 | " r = R*utraj[k]\n", 198 | " \n", 199 | " A = dfdx(xtraj[:,k], utraj[k]) #ForwardDiff.jacobian(dx->dynamics_rk4(dx,utraj[k]),xtraj[:,k])\n", 200 | " B = dfdu(xtraj[:,k], utraj[k]) #ForwardDiff.derivative(du->dynamics_rk4(xtraj[:,k],du),utraj[k])\n", 201 | " \n", 202 | " gx = q + A'*p[:,k+1]\n", 203 | " gu = r + B'*p[:,k+1]\n", 204 | " \n", 205 | " Gxx = Q + A'*P[:,:,k+1]*A \n", 206 | " Guu = R + B'*P[:,:,k+1]*B \n", 207 | " Gxu = A'*P[:,:,k+1]*B \n", 208 | " Gux = B'*P[:,:,k+1]*A\n", 209 | " \n", 210 | " d[k] = Guu\\gu\n", 211 | " K[:,:,k] .= Guu\\Gux\n", 212 | " \n", 213 | " p[:,k] .= dropdims(gx - K[:,:,k]'*gu + K[:,:,k]'*Guu*d[k] - Gxu*d[k], dims=2)\n", 214 | " P[:,:,k] .= Gxx + K[:,:,k]'*Guu*K[:,:,k] - Gxu*K[:,:,k] - K[:,:,k]'*Gux\n", 215 | " \n", 216 | " ΔJ += gu'*d[k]\n", 217 | " end\n", 218 | "\n", 219 | " #Forward rollout with line search\n", 220 | " xn[:,1] = xtraj[:,1]\n", 221 | " α = 1.0\n", 222 | "\n", 223 | " for k = 1:(Nt-1)\n", 224 | " un[k] = utraj[k] - α*d[k] - dot(K[:,:,k],xn[:,k]-xtraj[:,k])\n", 225 | " xn[:,k+1] .= dynamics_rk4(xn[:,k],un[k])\n", 226 | " end\n", 227 | " Jn = cost(xn,un)\n", 228 | " \n", 229 | " while isnan(Jn) || Jn > (J - 1e-2*α*ΔJ)\n", 230 | " α = 0.5*α\n", 231 | " for k = 1:(Nt-1)\n", 232 | " un[k] = utraj[k] - α*d[k] - dot(K[:,:,k],xn[:,k]-xtraj[:,k])\n", 233 | " xn[:,k+1] .= dynamics_rk4(xn[:,k],un[k])\n", 234 | " end\n", 235 | " Jn = cost(xn,un)\n", 236 | " end\n", 237 | " #display(α)\n", 238 | " \n", 239 | " J = Jn\n", 240 | " xtraj .= xn\n", 241 | " utraj .= un\n", 242 | "end" 243 | ] 244 | }, 245 | { 246 | "cell_type": "code", 247 | "execution_count": null, 248 | "metadata": {}, 249 | "outputs": [], 250 | "source": [ 251 | "iter" 252 | ] 253 | }, 254 | { 255 | "cell_type": "code", 256 | "execution_count": null, 257 | "metadata": {}, 258 | "outputs": [], 259 | "source": [ 260 | "J" 261 | ] 262 | }, 263 | { 264 | "cell_type": "code", 265 | "execution_count": null, 266 | "metadata": {}, 267 | "outputs": [], 268 | "source": [ 269 | "plot(thist,xtraj[1,:])\n", 270 | "plot(thist,xtraj[2,:])" 271 | ] 272 | }, 273 | { 274 | "cell_type": "code", 275 | "execution_count": null, 276 | "metadata": {}, 277 | "outputs": [], 278 | "source": [ 279 | "plot(thist[1:Nt-1],utraj)" 280 | ] 281 | }, 282 | { 283 | "cell_type": "code", 284 | "execution_count": null, 285 | "metadata": {}, 286 | "outputs": [], 287 | "source": [ 288 | "using TrajOptPlots\n", 289 | "using MeshCat\n", 290 | "using StaticArrays\n", 291 | "\n", 292 | "vis = Visualizer()\n", 293 | "render(vis)" 294 | ] 295 | }, 296 | { 297 | "cell_type": "code", 298 | "execution_count": null, 299 | "metadata": {}, 300 | "outputs": [], 301 | "source": [ 302 | "TrajOptPlots.set_mesh!(vis, a)" 303 | ] 304 | }, 305 | { 306 | "cell_type": "code", 307 | "execution_count": null, 308 | "metadata": {}, 309 | "outputs": [], 310 | "source": [ 311 | "X1 = [SVector{4}(x) for x in eachcol(xtraj)];\n", 312 | "visualize!(vis, a, thist[end], X1)" 313 | ] 314 | }, 315 | { 316 | "cell_type": "code", 317 | "execution_count": null, 318 | "metadata": {}, 319 | "outputs": [], 320 | "source": [] 321 | } 322 | ], 323 | "metadata": { 324 | "kernelspec": { 325 | "display_name": "Julia 1.6.7", 326 | "language": "julia", 327 | "name": "julia-1.6" 328 | }, 329 | "language_info": { 330 | "file_extension": ".jl", 331 | "mimetype": "application/julia", 332 | "name": "julia", 333 | "version": "1.6.7" 334 | } 335 | }, 336 | "nbformat": 4, 337 | "nbformat_minor": 4 338 | } 339 | -------------------------------------------------------------------------------- /Lecture 11/Lecture 11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 11/Lecture 11.pdf -------------------------------------------------------------------------------- /Lecture 11/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | Altro = "5dcf52e5-e2fb-48e0-b826-96f46d2e3e73" 3 | JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819" 4 | MatrixCalculus = "fafa779c-62f8-11e9-0c41-4d0261472a5f" 5 | MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11" 6 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 7 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 8 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 9 | TrajOptPlots = "7770976a-8dee-4930-bf39-a1782fd21ce6" 10 | TrajectoryOptimization = "c79d492b-0548-5874-b488-5a62c1d9d0ca" 11 | -------------------------------------------------------------------------------- /Lecture 11/acrobot.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 RobotZoo\n", 22 | "using RobotDynamics\n", 23 | "using MatrixCalculus\n", 24 | "using JLD2" 25 | ] 26 | }, 27 | { 28 | "cell_type": "code", 29 | "execution_count": null, 30 | "metadata": {}, 31 | "outputs": [], 32 | "source": [ 33 | "#Acrobot Dynamics\n", 34 | "a = RobotZoo.Acrobot()\n", 35 | "h = 0.05" 36 | ] 37 | }, 38 | { 39 | "cell_type": "code", 40 | "execution_count": null, 41 | "metadata": {}, 42 | "outputs": [], 43 | "source": [ 44 | "function dynamics_rk4(x,u)\n", 45 | " #RK4 integration with zero-order hold on u\n", 46 | " f1 = dynamics(a, x, u)\n", 47 | " f2 = dynamics(a, x + 0.5*h*f1, u)\n", 48 | " f3 = dynamics(a, x + 0.5*h*f2, u)\n", 49 | " f4 = dynamics(a, x + h*f3, u)\n", 50 | " return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 51 | "end" 52 | ] 53 | }, 54 | { 55 | "cell_type": "code", 56 | "execution_count": null, 57 | "metadata": {}, 58 | "outputs": [], 59 | "source": [ 60 | "function dfdx(x,u)\n", 61 | " ForwardDiff.jacobian(dx->dynamics_rk4(dx,u),x)\n", 62 | "end\n", 63 | "\n", 64 | "function dfdu(x,u)\n", 65 | " ForwardDiff.derivative(du->dynamics_rk4(x,du),u)\n", 66 | "end" 67 | ] 68 | }, 69 | { 70 | "cell_type": "code", 71 | "execution_count": null, 72 | "metadata": {}, 73 | "outputs": [], 74 | "source": [ 75 | "function dAdx(x,u)\n", 76 | " ForwardDiff.jacobian(dx->vec(dfdx(dx,u)),x)\n", 77 | "end\n", 78 | "\n", 79 | "function dBdx(x,u)\n", 80 | " ForwardDiff.jacobian(dx->dfdu(dx,u),x)\n", 81 | "end\n", 82 | "\n", 83 | "function dAdu(x,u)\n", 84 | " ForwardDiff.derivative(du->vec(dfdx(x,du)),u)\n", 85 | "end\n", 86 | "\n", 87 | "function dBdu(x,u)\n", 88 | " ForwardDiff.derivative(du->dfdu(x,du),u)\n", 89 | "end" 90 | ] 91 | }, 92 | { 93 | "cell_type": "code", 94 | "execution_count": null, 95 | "metadata": {}, 96 | "outputs": [], 97 | "source": [ 98 | "Nx = 4 # number of state\n", 99 | "Nu = 1 # number of controls\n", 100 | "Tfinal = 10.0 # final time\n", 101 | "Nt = Int(Tfinal/h)+1 # number of time steps\n", 102 | "thist = Array(range(0,h*(Nt-1), step=h));" 103 | ] 104 | }, 105 | { 106 | "cell_type": "code", 107 | "execution_count": null, 108 | "metadata": {}, 109 | "outputs": [], 110 | "source": [ 111 | "# Cost weights\n", 112 | "Q = Diagonal([1.0*ones(2); 0.1*ones(2)]);\n", 113 | "R = 0.01;\n", 114 | "Qn = Array(100.0*I(Nx));" 115 | ] 116 | }, 117 | { 118 | "cell_type": "code", 119 | "execution_count": null, 120 | "metadata": {}, 121 | "outputs": [], 122 | "source": [ 123 | "function stage_cost(x,u)\n", 124 | " return 0.5*((x-xgoal)'*Q*(x-xgoal)) + 0.5*R*u*u\n", 125 | "end" 126 | ] 127 | }, 128 | { 129 | "cell_type": "code", 130 | "execution_count": null, 131 | "metadata": {}, 132 | "outputs": [], 133 | "source": [ 134 | "function terminal_cost(x)\n", 135 | " return 0.5*(x-xgoal)'*Qn*(x-xgoal)\n", 136 | "end" 137 | ] 138 | }, 139 | { 140 | "cell_type": "code", 141 | "execution_count": null, 142 | "metadata": {}, 143 | "outputs": [], 144 | "source": [ 145 | "function cost(xtraj,utraj)\n", 146 | " J = 0.0\n", 147 | " for k = 1:(Nt-1)\n", 148 | " J += stage_cost(xtraj[:,k],utraj[k])\n", 149 | " end\n", 150 | " J += terminal_cost(xtraj[:,Nt])\n", 151 | " return J\n", 152 | "end" 153 | ] 154 | }, 155 | { 156 | "cell_type": "code", 157 | "execution_count": null, 158 | "metadata": {}, 159 | "outputs": [], 160 | "source": [ 161 | "function backward_pass!(p,P,d,K)\n", 162 | " \n", 163 | " ΔJ = 0.0\n", 164 | " p[:,Nt] .= Qn*(xtraj[:,Nt]-xgoal)\n", 165 | " P[:,:,Nt] .= Qn\n", 166 | " \n", 167 | " for k = (Nt-1):-1:1\n", 168 | " #Calculate derivatives\n", 169 | " q = Q*(xtraj[:,k]-xgoal)\n", 170 | " r = R*utraj[k]\n", 171 | " \n", 172 | " A = dfdx(xtraj[:,k], utraj[k]) #ForwardDiff.jacobian(dx->dynamics_rk4(dx,utraj[k]),xtraj[:,k])\n", 173 | " B = dfdu(xtraj[:,k], utraj[k]) #ForwardDiff.derivative(du->dynamics_rk4(xtraj[:,k],du),utraj[k])\n", 174 | " \n", 175 | " #Ax = dAdx(xtraj[:,k], utraj[k])\n", 176 | " #Bx = dBdx(xtraj[:,k], utraj[k])\n", 177 | " #Au = dAdu(xtraj[:,k], utraj[k])\n", 178 | " #Bu = dBdu(xtraj[:,k], utraj[k])\n", 179 | " \n", 180 | " gx = q + A'*p[:,k+1]\n", 181 | " gu = r + B'*p[:,k+1]\n", 182 | " \n", 183 | " #iLQR (Gauss-Newton) version\n", 184 | " Gxx = Q + A'*P[:,:,k+1]*A\n", 185 | " Guu = R + B'*P[:,:,k+1]*B\n", 186 | " Gxu = A'*P[:,:,k+1]*B\n", 187 | " Gux = B'*P[:,:,k+1]*A\n", 188 | " \n", 189 | " #DDP (full Newton) version\n", 190 | " #Gxx = Q + A'*P[:,:,k+1]*A + kron(p[:,k+1]',I(Nx))*comm(Nx,Nx)*Ax\n", 191 | " #Guu = R + B'*P[:,:,k+1]*B + (kron(p[:,k+1]',I(Nu))*comm(Nx,Nu)*Bu)[1]\n", 192 | " #Gxu = A'*P[:,:,k+1]*B + kron(p[:,k+1]',I(Nx))*comm(Nx,Nx)*Au\n", 193 | " #Gux = B'*P[:,:,k+1]*A + kron(p[:,k+1]',I(Nu))*comm(Nx,Nu)*Bx\n", 194 | " \n", 195 | " β = 0.1\n", 196 | " while !isposdef(Symmetric([Gxx Gxu; Gux Guu]))\n", 197 | " Gxx += A'*β*I*A\n", 198 | " Guu += B'*β*I*B\n", 199 | " Gxu += A'*β*I*B\n", 200 | " Gux += B'*β*I*A\n", 201 | " β = 2*β\n", 202 | " #display(\"regularizing G\")\n", 203 | " #display(β)\n", 204 | " end\n", 205 | " \n", 206 | " d[k] = Guu\\gu\n", 207 | " K[:,:,k] .= Guu\\Gux\n", 208 | " \n", 209 | " p[:,k] .= dropdims(gx - K[:,:,k]'*gu + K[:,:,k]'*Guu*d[k] - Gxu*d[k], dims=2)\n", 210 | " P[:,:,k] .= Gxx + K[:,:,k]'*Guu*K[:,:,k] - Gxu*K[:,:,k] - K[:,:,k]'*Gux\n", 211 | " \n", 212 | " ΔJ += gu'*d[k]\n", 213 | " end\n", 214 | " \n", 215 | " return ΔJ\n", 216 | "end" 217 | ] 218 | }, 219 | { 220 | "cell_type": "code", 221 | "execution_count": null, 222 | "metadata": {}, 223 | "outputs": [], 224 | "source": [ 225 | "#Initial guess\n", 226 | "x0 = [-pi/2; 0; 0; 0]\n", 227 | "xgoal = [pi/2; 0; 0; 0]\n", 228 | "xtraj = kron(ones(1,Nt), x0)\n", 229 | "utraj = randn(Nt-1);\n", 230 | "#f = jldopen(\"guess.jld2\", \"r\")\n", 231 | "#utraj = f[\"utraj\"];" 232 | ] 233 | }, 234 | { 235 | "cell_type": "code", 236 | "execution_count": null, 237 | "metadata": {}, 238 | "outputs": [], 239 | "source": [ 240 | "#Initial Rollout\n", 241 | "for k = 1:(Nt-1)\n", 242 | " xtraj[:,k+1] .= dynamics_rk4(xtraj[:,k],utraj[k])\n", 243 | "end\n", 244 | "J = cost(xtraj,utraj)" 245 | ] 246 | }, 247 | { 248 | "cell_type": "code", 249 | "execution_count": null, 250 | "metadata": {}, 251 | "outputs": [], 252 | "source": [ 253 | "#DDP Algorithm\n", 254 | "p = ones(Nx,Nt)\n", 255 | "P = zeros(Nx,Nx,Nt)\n", 256 | "d = ones(Nt-1)\n", 257 | "K = zeros(Nu,Nx,Nt-1)\n", 258 | "ΔJ = 1.0\n", 259 | "\n", 260 | "xn = zeros(Nx,Nt)\n", 261 | "un = zeros(Nt-1)\n", 262 | "\n", 263 | "gx = zeros(Nx)\n", 264 | "gu = 0.0\n", 265 | "Gxx = zeros(Nx,Nx)\n", 266 | "Guu = 0.0\n", 267 | "Gxu = zeros(Nx)\n", 268 | "Gux = zeros(Nx)\n", 269 | "\n", 270 | "iter = 0\n", 271 | "while maximum(abs.(d[:])) > 1e-3\n", 272 | " iter += 1 \n", 273 | " \n", 274 | " #Backward Pass\n", 275 | " ΔJ = backward_pass!(p,P,d,K)\n", 276 | "\n", 277 | " #Forward rollout with line search\n", 278 | " xn[:,1] = xtraj[:,1]\n", 279 | " α = 1.0\n", 280 | "\n", 281 | " for k = 1:(Nt-1)\n", 282 | " un[k] = utraj[k] - α*d[k] - dot(K[:,:,k],xn[:,k]-xtraj[:,k])\n", 283 | " xn[:,k+1] .= dynamics_rk4(xn[:,k],un[k])\n", 284 | " end\n", 285 | " Jn = cost(xn,un)\n", 286 | " \n", 287 | " while isnan(Jn) || Jn > (J - 1e-2*α*ΔJ)\n", 288 | " α = 0.5*α\n", 289 | " for k = 1:(Nt-1)\n", 290 | " un[k] = utraj[k] - α*d[k] - dot(K[:,:,k],xn[:,k]-xtraj[:,k])\n", 291 | " xn[:,k+1] .= dynamics_rk4(xn[:,k],un[k])\n", 292 | " end\n", 293 | " Jn = cost(xn,un)\n", 294 | " end\n", 295 | " \n", 296 | " J = Jn\n", 297 | " xtraj .= xn\n", 298 | " utraj .= un\n", 299 | " \n", 300 | " #display(iter)\n", 301 | " #display(α)\n", 302 | " #display(maximum(abs.(d[:])))\n", 303 | "end" 304 | ] 305 | }, 306 | { 307 | "cell_type": "code", 308 | "execution_count": null, 309 | "metadata": {}, 310 | "outputs": [], 311 | "source": [ 312 | "iter" 313 | ] 314 | }, 315 | { 316 | "cell_type": "code", 317 | "execution_count": null, 318 | "metadata": {}, 319 | "outputs": [], 320 | "source": [ 321 | "J" 322 | ] 323 | }, 324 | { 325 | "cell_type": "code", 326 | "execution_count": null, 327 | "metadata": {}, 328 | "outputs": [], 329 | "source": [ 330 | "plot(thist,xtraj[1,:])\n", 331 | "plot(thist,xtraj[2,:])" 332 | ] 333 | }, 334 | { 335 | "cell_type": "code", 336 | "execution_count": null, 337 | "metadata": {}, 338 | "outputs": [], 339 | "source": [ 340 | "plot(thist[1:Nt-1],utraj)" 341 | ] 342 | }, 343 | { 344 | "cell_type": "code", 345 | "execution_count": null, 346 | "metadata": {}, 347 | "outputs": [], 348 | "source": [ 349 | "using TrajOptPlots\n", 350 | "using MeshCat\n", 351 | "using StaticArrays\n", 352 | "\n", 353 | "vis = Visualizer()\n", 354 | "render(vis)" 355 | ] 356 | }, 357 | { 358 | "cell_type": "code", 359 | "execution_count": null, 360 | "metadata": {}, 361 | "outputs": [], 362 | "source": [ 363 | "TrajOptPlots.set_mesh!(vis, a)" 364 | ] 365 | }, 366 | { 367 | "cell_type": "code", 368 | "execution_count": null, 369 | "metadata": {}, 370 | "outputs": [], 371 | "source": [ 372 | "X1 = [SVector{4}(x) for x in eachcol(xtraj)];\n", 373 | "visualize!(vis, a, thist[end], X1)" 374 | ] 375 | }, 376 | { 377 | "cell_type": "code", 378 | "execution_count": null, 379 | "metadata": {}, 380 | "outputs": [], 381 | "source": [] 382 | } 383 | ], 384 | "metadata": { 385 | "kernelspec": { 386 | "display_name": "Julia 1.6.7", 387 | "language": "julia", 388 | "name": "julia-1.6" 389 | }, 390 | "language_info": { 391 | "file_extension": ".jl", 392 | "mimetype": "application/julia", 393 | "name": "julia", 394 | "version": "1.6.7" 395 | } 396 | }, 397 | "nbformat": 4, 398 | "nbformat_minor": 4 399 | } 400 | -------------------------------------------------------------------------------- /Lecture 11/cartpole.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 RobotZoo\n", 22 | "using RobotDynamics\n", 23 | "using MatrixCalculus" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": null, 29 | "metadata": {}, 30 | "outputs": [], 31 | "source": [ 32 | "#Cartpole Dynamics\n", 33 | "a = RobotZoo.Cartpole()\n", 34 | "h = 0.05" 35 | ] 36 | }, 37 | { 38 | "cell_type": "code", 39 | "execution_count": null, 40 | "metadata": {}, 41 | "outputs": [], 42 | "source": [ 43 | "function dynamics_rk4(x,u)\n", 44 | " #RK4 integration with zero-order hold on u\n", 45 | " f1 = dynamics(a, x, u)\n", 46 | " f2 = dynamics(a, x + 0.5*h*f1, u)\n", 47 | " f3 = dynamics(a, x + 0.5*h*f2, u)\n", 48 | " f4 = dynamics(a, x + h*f3, u)\n", 49 | " return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 50 | "end" 51 | ] 52 | }, 53 | { 54 | "cell_type": "code", 55 | "execution_count": null, 56 | "metadata": {}, 57 | "outputs": [], 58 | "source": [ 59 | "function dfdx(x,u)\n", 60 | " ForwardDiff.jacobian(dx->dynamics_rk4(dx,u),x)\n", 61 | "end\n", 62 | "\n", 63 | "function dfdu(x,u)\n", 64 | " ForwardDiff.derivative(du->dynamics_rk4(x,du),u)\n", 65 | "end" 66 | ] 67 | }, 68 | { 69 | "cell_type": "code", 70 | "execution_count": null, 71 | "metadata": {}, 72 | "outputs": [], 73 | "source": [ 74 | "function dAdx(x,u)\n", 75 | " ForwardDiff.jacobian(dx->vec(dfdx(dx,u)),x)\n", 76 | "end\n", 77 | "\n", 78 | "function dBdx(x,u)\n", 79 | " ForwardDiff.jacobian(dx->dfdu(dx,u),x)\n", 80 | "end\n", 81 | "\n", 82 | "function dAdu(x,u)\n", 83 | " ForwardDiff.derivative(du->vec(dfdx(x,du)),u)\n", 84 | "end\n", 85 | "\n", 86 | "function dBdu(x,u)\n", 87 | " ForwardDiff.derivative(du->dfdu(x,du),u)\n", 88 | "end" 89 | ] 90 | }, 91 | { 92 | "cell_type": "code", 93 | "execution_count": null, 94 | "metadata": {}, 95 | "outputs": [], 96 | "source": [ 97 | "Nx = 4 # number of state\n", 98 | "Nu = 1 # number of controls\n", 99 | "Tfinal = 5.0 # final time\n", 100 | "Nt = Int(Tfinal/h)+1 # number of time steps\n", 101 | "thist = Array(range(0,h*(Nt-1), step=h));" 102 | ] 103 | }, 104 | { 105 | "cell_type": "code", 106 | "execution_count": null, 107 | "metadata": {}, 108 | "outputs": [], 109 | "source": [ 110 | "# Cost weights\n", 111 | "Q = Diagonal([1.0*ones(2); 1.0*ones(2)]);\n", 112 | "R = 0.1;\n", 113 | "Qn = Array(100.0*I(Nx));" 114 | ] 115 | }, 116 | { 117 | "cell_type": "code", 118 | "execution_count": null, 119 | "metadata": {}, 120 | "outputs": [], 121 | "source": [ 122 | "function stage_cost(x,u)\n", 123 | " return 0.5*((x-xgoal)'*Q*(x-xgoal)) + 0.5*R*u*u\n", 124 | "end" 125 | ] 126 | }, 127 | { 128 | "cell_type": "code", 129 | "execution_count": null, 130 | "metadata": {}, 131 | "outputs": [], 132 | "source": [ 133 | "function terminal_cost(x)\n", 134 | " return 0.5*(x-xgoal)'*Qn*(x-xgoal)\n", 135 | "end" 136 | ] 137 | }, 138 | { 139 | "cell_type": "code", 140 | "execution_count": null, 141 | "metadata": {}, 142 | "outputs": [], 143 | "source": [ 144 | "function cost(xtraj,utraj)\n", 145 | " J = 0.0\n", 146 | " for k = 1:(Nt-1)\n", 147 | " J += stage_cost(xtraj[:,k],utraj[k])\n", 148 | " end\n", 149 | " J += terminal_cost(xtraj[:,Nt])\n", 150 | " return J\n", 151 | "end" 152 | ] 153 | }, 154 | { 155 | "cell_type": "code", 156 | "execution_count": null, 157 | "metadata": {}, 158 | "outputs": [ 159 | { 160 | "data": { 161 | "text/plain": [ 162 | "backward_pass! (generic function with 1 method)" 163 | ] 164 | }, 165 | "execution_count": 1, 166 | "metadata": {}, 167 | "output_type": "execute_result" 168 | } 169 | ], 170 | "source": [ 171 | "function backward_pass!(p,P,d,K)\n", 172 | " \n", 173 | " ΔJ = 0.0\n", 174 | " p[:,Nt] .= Qn*(xtraj[:,Nt]-xgoal)\n", 175 | " P[:,:,Nt] .= Qn\n", 176 | " \n", 177 | " for k = (Nt-1):-1:1\n", 178 | " #Calculate derivatives\n", 179 | " q = Q*(xtraj[:,k]-xgoal)\n", 180 | " r = R*utraj[k]\n", 181 | " \n", 182 | " A = dfdx(xtraj[:,k], utraj[k]) #ForwardDiff.jacobian(dx->dynamics_rk4(dx,utraj[k]),xtraj[:,k])\n", 183 | " B = dfdu(xtraj[:,k], utraj[k]) #ForwardDiff.derivative(du->dynamics_rk4(xtraj[:,k],du),utraj[k])\n", 184 | " \n", 185 | " #Ax = dAdx(xtraj[:,k], utraj[k])\n", 186 | " #Bx = dBdx(xtraj[:,k], utraj[k])\n", 187 | " #Au = dAdu(xtraj[:,k], utraj[k])\n", 188 | " #Bu = dBdu(xtraj[:,k], utraj[k])\n", 189 | " \n", 190 | " gx = q + A'*p[:,k+1]\n", 191 | " gu = r + B'*p[:,k+1]\n", 192 | " \n", 193 | " #iLQR (Gauss-Newton) version\n", 194 | " Gxx = Q + A'*P[:,:,k+1]*A\n", 195 | " Guu = R + B'*P[:,:,k+1]*B\n", 196 | " Gxu = A'*P[:,:,k+1]*B\n", 197 | " Gux = B'*P[:,:,k+1]*A\n", 198 | " \n", 199 | " #DDP (full Newton) version\n", 200 | " #Gxx = Q + A'*P[:,:,k+1]*A + kron(p[:,k+1]',I(Nx))*comm(Nx,Nx)*Ax\n", 201 | " #Guu = R + B'*P[:,:,k+1]*B + (kron(p[:,k+1]',I(Nu))*comm(Nx,Nu)*Bu)[1]\n", 202 | " #Gxu = A'*P[:,:,k+1]*B + kron(p[:,k+1]',I(Nx))*comm(Nx,Nx)*Au\n", 203 | " #Gux = B'*P[:,:,k+1]*A + kron(p[:,k+1]',I(Nu))*comm(Nx,Nu)*Bx\n", 204 | " \n", 205 | " β = 0.1\n", 206 | " while !isposdef(Symmetric([Gxx Gxu; Gux Guu]))\n", 207 | " Gxx += A'*β*I*A\n", 208 | " Guu += B'*β*I*B\n", 209 | " Gxu += A'*β*I*B\n", 210 | " Gux += B'*β*I*A\n", 211 | " β = 2*β\n", 212 | " display(\"regularizing G\")\n", 213 | " #display(β)\n", 214 | " end\n", 215 | " \n", 216 | " d[k] = Guu\\gu\n", 217 | " K[:,:,k] .= Guu\\Gux\n", 218 | " \n", 219 | " p[:,k] .= dropdims(gx - K[:,:,k]'*gu + K[:,:,k]'*Guu*d[k] - Gxu*d[k], dims=2)\n", 220 | " P[:,:,k] .= Gxx + K[:,:,k]'*Guu*K[:,:,k] - Gxu*K[:,:,k] - K[:,:,k]'*Gux\n", 221 | " \n", 222 | " ΔJ += gu'*d[k]\n", 223 | " end\n", 224 | " \n", 225 | " return ΔJ\n", 226 | "end" 227 | ] 228 | }, 229 | { 230 | "cell_type": "code", 231 | "execution_count": null, 232 | "metadata": {}, 233 | "outputs": [], 234 | "source": [ 235 | "#Initial guess\n", 236 | "x0 = [0; 0; 0; 0]\n", 237 | "xgoal = [0, pi, 0, 0]\n", 238 | "xtraj = kron(ones(1,Nt), x0)\n", 239 | "utraj = 0.001*ones(Nt-1);" 240 | ] 241 | }, 242 | { 243 | "cell_type": "code", 244 | "execution_count": null, 245 | "metadata": {}, 246 | "outputs": [], 247 | "source": [ 248 | "#Initial Rollout\n", 249 | "for k = 1:(Nt-1)\n", 250 | " xtraj[:,k+1] .= dynamics_rk4(xtraj[:,k],utraj[k])\n", 251 | "end\n", 252 | "J = cost(xtraj,utraj)" 253 | ] 254 | }, 255 | { 256 | "cell_type": "code", 257 | "execution_count": null, 258 | "metadata": {}, 259 | "outputs": [], 260 | "source": [ 261 | "#DDP Algorithm\n", 262 | "p = ones(Nx,Nt)\n", 263 | "P = zeros(Nx,Nx,Nt)\n", 264 | "d = ones(Nt-1)\n", 265 | "K = zeros(Nu,Nx,Nt-1)\n", 266 | "ΔJ = 1.0\n", 267 | "\n", 268 | "xn = zeros(Nx,Nt)\n", 269 | "un = zeros(Nt-1)\n", 270 | "\n", 271 | "gx = zeros(Nx)\n", 272 | "gu = 0.0\n", 273 | "Gxx = zeros(Nx,Nx)\n", 274 | "Guu = 0.0\n", 275 | "Gxu = zeros(Nx)\n", 276 | "Gux = zeros(Nx)\n", 277 | "\n", 278 | "iter = 0\n", 279 | "while maximum(abs.(d[:])) > 1e-3\n", 280 | " iter += 1 \n", 281 | " \n", 282 | " #Backward Pass\n", 283 | " ΔJ = backward_pass!(p,P,d,K)\n", 284 | "\n", 285 | " #Forward rollout with line search\n", 286 | " xn[:,1] = xtraj[:,1]\n", 287 | " α = 1.0\n", 288 | "\n", 289 | " for k = 1:(Nt-1)\n", 290 | " un[k] = utraj[k] - α*d[k] - dot(K[:,:,k],xn[:,k]-xtraj[:,k])\n", 291 | " xn[:,k+1] .= dynamics_rk4(xn[:,k],un[k])\n", 292 | " end\n", 293 | " Jn = cost(xn,un)\n", 294 | " \n", 295 | " while isnan(Jn) || Jn > (J - 1e-2*α*ΔJ)\n", 296 | " α = 0.5*α\n", 297 | " for k = 1:(Nt-1)\n", 298 | " un[k] = utraj[k] - α*d[k] - dot(K[:,:,k],xn[:,k]-xtraj[:,k])\n", 299 | " xn[:,k+1] .= dynamics_rk4(xn[:,k],un[k])\n", 300 | " end\n", 301 | " Jn = cost(xn,un)\n", 302 | " end\n", 303 | " \n", 304 | " J = Jn\n", 305 | " xtraj .= xn\n", 306 | " utraj .= un\n", 307 | "end" 308 | ] 309 | }, 310 | { 311 | "cell_type": "code", 312 | "execution_count": null, 313 | "metadata": {}, 314 | "outputs": [], 315 | "source": [ 316 | "iter" 317 | ] 318 | }, 319 | { 320 | "cell_type": "code", 321 | "execution_count": null, 322 | "metadata": {}, 323 | "outputs": [], 324 | "source": [ 325 | "J" 326 | ] 327 | }, 328 | { 329 | "cell_type": "code", 330 | "execution_count": null, 331 | "metadata": {}, 332 | "outputs": [], 333 | "source": [ 334 | "plot(thist,xtraj[1,:])\n", 335 | "plot(thist,xtraj[2,:])" 336 | ] 337 | }, 338 | { 339 | "cell_type": "code", 340 | "execution_count": null, 341 | "metadata": {}, 342 | "outputs": [], 343 | "source": [ 344 | "plot(thist[1:Nt-1],utraj)" 345 | ] 346 | }, 347 | { 348 | "cell_type": "code", 349 | "execution_count": null, 350 | "metadata": {}, 351 | "outputs": [], 352 | "source": [ 353 | "using TrajOptPlots\n", 354 | "using MeshCat\n", 355 | "using StaticArrays\n", 356 | "\n", 357 | "vis = Visualizer()\n", 358 | "render(vis)" 359 | ] 360 | }, 361 | { 362 | "cell_type": "code", 363 | "execution_count": null, 364 | "metadata": {}, 365 | "outputs": [], 366 | "source": [ 367 | "TrajOptPlots.set_mesh!(vis, a)" 368 | ] 369 | }, 370 | { 371 | "cell_type": "code", 372 | "execution_count": null, 373 | "metadata": {}, 374 | "outputs": [], 375 | "source": [ 376 | "X1 = [SVector{4}(x) for x in eachcol(xtraj)];\n", 377 | "visualize!(vis, a, thist[end], X1)" 378 | ] 379 | }, 380 | { 381 | "cell_type": "code", 382 | "execution_count": null, 383 | "metadata": {}, 384 | "outputs": [], 385 | "source": [] 386 | } 387 | ], 388 | "metadata": { 389 | "kernelspec": { 390 | "display_name": "Julia 1.6.7", 391 | "language": "julia", 392 | "name": "julia-1.6" 393 | }, 394 | "language_info": { 395 | "file_extension": ".jl", 396 | "mimetype": "application/julia", 397 | "name": "julia", 398 | "version": "1.6.7" 399 | } 400 | }, 401 | "nbformat": 4, 402 | "nbformat_minor": 4 403 | } 404 | -------------------------------------------------------------------------------- /Lecture 11/guess.jld2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 11/guess.jld2 -------------------------------------------------------------------------------- /Lecture 12/Lecture 12.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 12/Lecture 12.pdf -------------------------------------------------------------------------------- /Lecture 12/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" 3 | MathOptInterface = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" 4 | MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11" 5 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 6 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 7 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 8 | TrajOptPlots = "7770976a-8dee-4930-bf39-a1782fd21ce6" 9 | -------------------------------------------------------------------------------- /Lecture 12/dircol.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": { 7 | "tags": [] 8 | }, 9 | "outputs": [], 10 | "source": [ 11 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "using LinearAlgebra\n", 21 | "using PyPlot\n", 22 | "using ForwardDiff\n", 23 | "using RobotZoo\n", 24 | "const RZ = RobotZoo\n", 25 | "using RobotDynamics\n", 26 | "using Ipopt\n", 27 | "using MathOptInterface\n", 28 | "const MOI = MathOptInterface;" 29 | ] 30 | }, 31 | { 32 | "cell_type": "code", 33 | "execution_count": null, 34 | "metadata": {}, 35 | "outputs": [], 36 | "source": [ 37 | "#Pendulum Dynamics\n", 38 | "a = RobotZoo.Acrobot()\n", 39 | "h = 0.1 #10 Hz" 40 | ] 41 | }, 42 | { 43 | "cell_type": "code", 44 | "execution_count": null, 45 | "metadata": {}, 46 | "outputs": [], 47 | "source": [ 48 | "Nx = 4 # number of state\n", 49 | "Nu = 1 # number of controls\n", 50 | "Tfinal = 5.0 # final time\n", 51 | "Nt = Int(Tfinal/h)+1 # number of time steps\n", 52 | "thist = Array(range(0,h*(Nt-1), step=h));\n", 53 | "n_nlp = (Nx+Nu)*Nt # number of decision variables\n", 54 | "m_nlp = Nx*(Nt+1)" 55 | ] 56 | }, 57 | { 58 | "cell_type": "code", 59 | "execution_count": null, 60 | "metadata": {}, 61 | "outputs": [], 62 | "source": [ 63 | "function dircol_dynamics(x1,u1,x2,u2)\n", 64 | " #Hermite-Simpson integration with first-order hold on u\n", 65 | " f1 = RZ.dynamics(a, x1, u1)\n", 66 | " f2 = RZ.dynamics(a, x2, u2)\n", 67 | " xm = 0.5*(x1 + x2) + (h/8.0)*(f1 - f2)\n", 68 | " um = 0.5*(u1 + u2)\n", 69 | " ẋm = (-3/(2.0*h))*(x1 - x2) - 0.25*(f1 + f2)\n", 70 | " fm = RZ.dynamics(a, xm, um)\n", 71 | " return fm - ẋm\n", 72 | "end" 73 | ] 74 | }, 75 | { 76 | "cell_type": "code", 77 | "execution_count": null, 78 | "metadata": {}, 79 | "outputs": [], 80 | "source": [ 81 | "# Cost weights\n", 82 | "Q = Diagonal([1.0*ones(2); 1.0*ones(2)]);\n", 83 | "R = 0.1;" 84 | ] 85 | }, 86 | { 87 | "cell_type": "code", 88 | "execution_count": null, 89 | "metadata": {}, 90 | "outputs": [], 91 | "source": [ 92 | "function stage_cost(x,u)\n", 93 | " return 0.5*((x-xgoal)'*Q*(x-xgoal)) + 0.5*u'*R*u\n", 94 | "end" 95 | ] 96 | }, 97 | { 98 | "cell_type": "code", 99 | "execution_count": null, 100 | "metadata": {}, 101 | "outputs": [], 102 | "source": [ 103 | "function cost(ztraj)\n", 104 | " z = reshape(ztraj,Nx+Nu,Nt)\n", 105 | " J = 0.0\n", 106 | " for k = 1:Nt\n", 107 | " J += stage_cost(z[1:Nx,k],z[(Nx+1):(Nx+Nu),k])\n", 108 | " end\n", 109 | " return J\n", 110 | "end" 111 | ] 112 | }, 113 | { 114 | "cell_type": "code", 115 | "execution_count": null, 116 | "metadata": {}, 117 | "outputs": [], 118 | "source": [ 119 | "function dynamics_constraint!(c,ztraj)\n", 120 | " d = reshape(c,Nx,Nt-1)\n", 121 | " z = reshape(ztraj,Nx+Nu,Nt)\n", 122 | " for k = 1:(Nt-1)\n", 123 | " x1 = z[1:Nx,k]\n", 124 | " u1 = z[(Nx+1):(Nx+Nu),k]\n", 125 | " x2 = z[1:Nx,k+1]\n", 126 | " u2 = z[(Nx+1):(Nx+Nu),k+1]\n", 127 | " d[:,k] = dircol_dynamics(x1,u1,x2,u2)\n", 128 | " end\n", 129 | " return nothing\n", 130 | "end" 131 | ] 132 | }, 133 | { 134 | "cell_type": "code", 135 | "execution_count": null, 136 | "metadata": {}, 137 | "outputs": [], 138 | "source": [ 139 | "function con!(c,ztraj)\n", 140 | " z = reshape(ztraj,Nx+Nu,Nt)\n", 141 | " c[1:Nx] .= z[1:Nx,1] - x0\n", 142 | " @views dynamics_constraint!(c[(Nx+1):(end-Nx)],ztraj)\n", 143 | " c[(end-Nx+1):end] .= z[1:Nx,end] - xgoal\n", 144 | "end" 145 | ] 146 | }, 147 | { 148 | "cell_type": "code", 149 | "execution_count": null, 150 | "metadata": {}, 151 | "outputs": [], 152 | "source": [ 153 | "struct ProblemMOI <: MOI.AbstractNLPEvaluator\n", 154 | " n_nlp::Int\n", 155 | " m_nlp::Int\n", 156 | " idx_ineq\n", 157 | " obj_grad::Bool\n", 158 | " con_jac::Bool\n", 159 | " sparsity_jac\n", 160 | " sparsity_hess\n", 161 | " primal_bounds\n", 162 | " constraint_bounds\n", 163 | " hessian_lagrangian::Bool\n", 164 | "end\n", 165 | "\n", 166 | "function ProblemMOI(n_nlp,m_nlp;\n", 167 | " idx_ineq=(1:0),\n", 168 | " obj_grad=true,\n", 169 | " con_jac=true,\n", 170 | " sparsity_jac=sparsity_jacobian(n_nlp,m_nlp),\n", 171 | " sparsity_hess=sparsity_hessian(n_nlp,m_nlp),\n", 172 | " primal_bounds=primal_bounds(n_nlp),\n", 173 | " constraint_bounds=constraint_bounds(m_nlp,idx_ineq=idx_ineq),\n", 174 | " hessian_lagrangian=false)\n", 175 | "\n", 176 | " ProblemMOI(n_nlp,m_nlp,\n", 177 | " idx_ineq,\n", 178 | " obj_grad,\n", 179 | " con_jac,\n", 180 | " sparsity_jac,\n", 181 | " sparsity_hess,\n", 182 | " primal_bounds,\n", 183 | " constraint_bounds,\n", 184 | " hessian_lagrangian)\n", 185 | "end\n", 186 | "\n", 187 | "function primal_bounds(n)\n", 188 | " x_l = -Inf*ones(n)\n", 189 | " x_u = Inf*ones(n)\n", 190 | " return x_l, x_u\n", 191 | "end\n", 192 | "\n", 193 | "function constraint_bounds(m; idx_ineq=(1:0))\n", 194 | " c_l = zeros(m)\n", 195 | " c_l[idx_ineq] .= -Inf\n", 196 | "\n", 197 | " c_u = zeros(m)\n", 198 | " return c_l, c_u\n", 199 | "end\n", 200 | "\n", 201 | "function row_col!(row,col,r,c)\n", 202 | " for cc in c\n", 203 | " for rr in r\n", 204 | " push!(row,convert(Int,rr))\n", 205 | " push!(col,convert(Int,cc))\n", 206 | " end\n", 207 | " end\n", 208 | " return row, col\n", 209 | "end\n", 210 | "\n", 211 | "function sparsity_jacobian(n,m)\n", 212 | "\n", 213 | " row = []\n", 214 | " col = []\n", 215 | "\n", 216 | " r = 1:m\n", 217 | " c = 1:n\n", 218 | "\n", 219 | " row_col!(row,col,r,c)\n", 220 | "\n", 221 | " return collect(zip(row,col))\n", 222 | "end\n", 223 | "\n", 224 | "function sparsity_hessian(n,m)\n", 225 | "\n", 226 | " row = []\n", 227 | " col = []\n", 228 | "\n", 229 | " r = 1:m\n", 230 | " c = 1:n\n", 231 | "\n", 232 | " row_col!(row,col,r,c)\n", 233 | "\n", 234 | " return collect(zip(row,col))\n", 235 | "end\n", 236 | "\n", 237 | "function MOI.eval_objective(prob::MOI.AbstractNLPEvaluator, x)\n", 238 | " cost(x)\n", 239 | "end\n", 240 | "\n", 241 | "function MOI.eval_objective_gradient(prob::MOI.AbstractNLPEvaluator, grad_f, x)\n", 242 | " ForwardDiff.gradient!(grad_f,cost,x)\n", 243 | " return nothing\n", 244 | "end\n", 245 | "\n", 246 | "function MOI.eval_constraint(prob::MOI.AbstractNLPEvaluator,g,x)\n", 247 | " con!(g,x)\n", 248 | " return nothing\n", 249 | "end\n", 250 | "\n", 251 | "function MOI.eval_constraint_jacobian(prob::MOI.AbstractNLPEvaluator, jac, x)\n", 252 | " ForwardDiff.jacobian!(reshape(jac,prob.m_nlp,prob.n_nlp), con!, zeros(prob.m_nlp), x)\n", 253 | " return nothing\n", 254 | "end\n", 255 | "\n", 256 | "function MOI.features_available(prob::MOI.AbstractNLPEvaluator)\n", 257 | " return [:Grad, :Jac]\n", 258 | "end\n", 259 | "\n", 260 | "MOI.initialize(prob::MOI.AbstractNLPEvaluator, features) = nothing\n", 261 | "MOI.jacobian_structure(prob::MOI.AbstractNLPEvaluator) = prob.sparsity_jac\n", 262 | "\n", 263 | "function solve(x0,prob::MOI.AbstractNLPEvaluator;\n", 264 | " tol=1.0e-6,c_tol=1.0e-6,max_iter=10000)\n", 265 | " x_l, x_u = prob.primal_bounds\n", 266 | " c_l, c_u = prob.constraint_bounds\n", 267 | "\n", 268 | " nlp_bounds = MOI.NLPBoundsPair.(c_l,c_u)\n", 269 | " block_data = MOI.NLPBlockData(nlp_bounds,prob,true)\n", 270 | "\n", 271 | " solver = Ipopt.Optimizer()\n", 272 | " solver.options[\"max_iter\"] = max_iter\n", 273 | " solver.options[\"tol\"] = tol\n", 274 | " solver.options[\"constr_viol_tol\"] = c_tol\n", 275 | "\n", 276 | " x = MOI.add_variables(solver,prob.n_nlp)\n", 277 | "\n", 278 | " for i = 1:prob.n_nlp\n", 279 | " MOI.add_constraint(solver, x[i], MOI.LessThan(x_u[i]))\n", 280 | " MOI.add_constraint(solver, x[i], MOI.GreaterThan(x_l[i]))\n", 281 | " MOI.set(solver, MOI.VariablePrimalStart(), x[i], x0[i])\n", 282 | " end\n", 283 | "\n", 284 | " # Solve the problem\n", 285 | " MOI.set(solver, MOI.NLPBlock(), block_data)\n", 286 | " MOI.set(solver, MOI.ObjectiveSense(), MOI.MIN_SENSE)\n", 287 | " MOI.optimize!(solver)\n", 288 | "\n", 289 | " # Get the solution\n", 290 | " res = MOI.get(solver, MOI.VariablePrimal(), x)\n", 291 | "\n", 292 | " return res\n", 293 | "end" 294 | ] 295 | }, 296 | { 297 | "cell_type": "code", 298 | "execution_count": null, 299 | "metadata": {}, 300 | "outputs": [], 301 | "source": [ 302 | "#Initial and goal states\n", 303 | "x0 = [-pi/2; 0; 0; 0]\n", 304 | "xgoal = [pi/2; 0; 0; 0]" 305 | ] 306 | }, 307 | { 308 | "cell_type": "code", 309 | "execution_count": null, 310 | "metadata": {}, 311 | "outputs": [], 312 | "source": [ 313 | "#Guess\n", 314 | "xguess = kron(ones(Nt)', x0)\n", 315 | "xguess[1,:] .= xtraj[1,:] + 0.1*randn(Nt)\n", 316 | "uguess = zeros(Nt)'\n", 317 | "z0 = reshape([xguess; uguess],(Nx+Nu)*Nt,1);" 318 | ] 319 | }, 320 | { 321 | "cell_type": "code", 322 | "execution_count": null, 323 | "metadata": { 324 | "scrolled": true, 325 | "tags": [] 326 | }, 327 | "outputs": [], 328 | "source": [ 329 | "prob = ProblemMOI(n_nlp,m_nlp)\n", 330 | "z_sol = solve(z0,prob) # solve\n", 331 | "ztraj = reshape(z_sol,Nx+Nu,Nt)\n", 332 | "xtraj = ztraj[1:Nx,:]\n", 333 | "utraj = ztraj[(Nx+1):(Nx+Nu),:];" 334 | ] 335 | }, 336 | { 337 | "cell_type": "code", 338 | "execution_count": null, 339 | "metadata": {}, 340 | "outputs": [], 341 | "source": [ 342 | "plot(thist,xtraj[1,:])\n", 343 | "plot(thist,xtraj[2,:])\n", 344 | "plot(thist,xguess[1,:])" 345 | ] 346 | }, 347 | { 348 | "cell_type": "code", 349 | "execution_count": null, 350 | "metadata": {}, 351 | "outputs": [], 352 | "source": [ 353 | "plot(thist,xtraj[3,:])\n", 354 | "plot(thist,xtraj[4,:])" 355 | ] 356 | }, 357 | { 358 | "cell_type": "code", 359 | "execution_count": null, 360 | "metadata": {}, 361 | "outputs": [], 362 | "source": [ 363 | "plot(thist,utraj[1,:])" 364 | ] 365 | }, 366 | { 367 | "cell_type": "code", 368 | "execution_count": null, 369 | "metadata": {}, 370 | "outputs": [], 371 | "source": [ 372 | "using TrajOptPlots\n", 373 | "using MeshCat\n", 374 | "using StaticArrays\n", 375 | "\n", 376 | "vis = Visualizer()\n", 377 | "render(vis)" 378 | ] 379 | }, 380 | { 381 | "cell_type": "code", 382 | "execution_count": null, 383 | "metadata": {}, 384 | "outputs": [], 385 | "source": [ 386 | "TrajOptPlots.set_mesh!(vis, a)" 387 | ] 388 | }, 389 | { 390 | "cell_type": "code", 391 | "execution_count": null, 392 | "metadata": {}, 393 | "outputs": [], 394 | "source": [ 395 | "X1 = [SVector{4}(x) for x in eachcol(xtraj)];\n", 396 | "visualize!(vis, a, thist[end], X1)" 397 | ] 398 | }, 399 | { 400 | "cell_type": "code", 401 | "execution_count": null, 402 | "metadata": {}, 403 | "outputs": [], 404 | "source": [] 405 | } 406 | ], 407 | "metadata": { 408 | "kernelspec": { 409 | "display_name": "Julia 1.6.7", 410 | "language": "julia", 411 | "name": "julia-1.6" 412 | }, 413 | "language_info": { 414 | "file_extension": ".jl", 415 | "mimetype": "application/julia", 416 | "name": "julia", 417 | "version": "1.6.7" 418 | } 419 | }, 420 | "nbformat": 4, 421 | "nbformat_minor": 4 422 | } 423 | -------------------------------------------------------------------------------- /Lecture 13/Lecture 13.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 13/Lecture 13.pdf -------------------------------------------------------------------------------- /Lecture 14/Lecture 14.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/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-2023/573bce67fe0acc32633bad3d42276efc70628145/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.7", 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.7" 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-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 15/Lecture 15.pdf -------------------------------------------------------------------------------- /Lecture 15/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | BlockDiagonals = "0a1fb500-61f7-11e9-3c65-f5ef3456f9f0" 3 | ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e" 4 | MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11" 5 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 6 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 7 | TrajOptPlots = "7770976a-8dee-4930-bf39-a1782fd21ce6" 8 | -------------------------------------------------------------------------------- /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]/norm(x[4:7]) #normalize q just to be careful\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([1; 0; 0]); 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.7", 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.7" 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-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 16/Lecture 16.pdf -------------------------------------------------------------------------------- /Lecture 16/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" 3 | CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298" 4 | GeometryBasics = "5c1252a2-5f33-56bf-86c9-59e7332b4326" 5 | Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" 6 | MathOptInterface = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" 7 | MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11" 8 | Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" 9 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 10 | -------------------------------------------------------------------------------- /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 | " MOI.add_constraint(solver, x[i], MOI.LessThan(x_u[i]))\n", 430 | " MOI.add_constraint(solver, x[i], MOI.GreaterThan(x_l[i]))\n", 431 | " MOI.set(solver, MOI.VariablePrimalStart(), x[i], x0[i])\n", 432 | " end\n", 433 | "\n", 434 | " # Solve the problem\n", 435 | " MOI.set(solver, MOI.NLPBlock(), block_data)\n", 436 | " MOI.set(solver, MOI.ObjectiveSense(), MOI.MIN_SENSE)\n", 437 | " MOI.optimize!(solver)\n", 438 | "\n", 439 | " # Get the solution\n", 440 | " res = MOI.get(solver, MOI.VariablePrimal(), x)\n", 441 | "\n", 442 | " return res\n", 443 | "end" 444 | ] 445 | }, 446 | { 447 | "cell_type": "code", 448 | "execution_count": null, 449 | "metadata": {}, 450 | "outputs": [], 451 | "source": [ 452 | "#Guess\n", 453 | "xguess = xref + 0.1*randn(Nx,Nt)\n", 454 | "uguess = kron(ones(Nt-1)', uref) + 0.1*randn(Nu,Nt-1)\n", 455 | "z0 = [reshape([xguess[:,1:(Nt-1)]; uguess],(Nx+Nu)*(Nt-1),1); xguess[:,end]];" 456 | ] 457 | }, 458 | { 459 | "cell_type": "code", 460 | "execution_count": null, 461 | "metadata": {}, 462 | "outputs": [], 463 | "source": [ 464 | "prob = ProblemMOI(n_nlp,m_nlp)\n", 465 | "z_sol = solve(z0,prob) # solve" 466 | ] 467 | }, 468 | { 469 | "cell_type": "code", 470 | "execution_count": null, 471 | "metadata": {}, 472 | "outputs": [], 473 | "source": [ 474 | "z = reshape(z_sol[1:(end-Nx)],Nx+Nu,Nt-1)\n", 475 | "xtraj = [z[1:Nx,:] z_sol[end-(Nx-1):end]]\n", 476 | "utraj = z[(Nx+1):(Nx+Nu),:];" 477 | ] 478 | }, 479 | { 480 | "cell_type": "code", 481 | "execution_count": null, 482 | "metadata": {}, 483 | "outputs": [], 484 | "source": [ 485 | "plot(thist,xtraj[2,:])\n", 486 | "plot(thist,xtraj[4,:])" 487 | ] 488 | }, 489 | { 490 | "cell_type": "code", 491 | "execution_count": null, 492 | "metadata": {}, 493 | "outputs": [], 494 | "source": [ 495 | "plot(thist,xtraj[1,:])\n", 496 | "plot(thist,xtraj[3,:])" 497 | ] 498 | }, 499 | { 500 | "cell_type": "code", 501 | "execution_count": null, 502 | "metadata": {}, 503 | "outputs": [], 504 | "source": [ 505 | "plot(thist[1:end-1],utraj[1,:])" 506 | ] 507 | }, 508 | { 509 | "cell_type": "code", 510 | "execution_count": null, 511 | "metadata": {}, 512 | "outputs": [], 513 | "source": [ 514 | "plot(thist[1:end-1],utraj[2,:])" 515 | ] 516 | }, 517 | { 518 | "cell_type": "code", 519 | "execution_count": null, 520 | "metadata": {}, 521 | "outputs": [], 522 | "source": [ 523 | "#Set up visualizer\n", 524 | "using Colors\n", 525 | "using CoordinateTransformations\n", 526 | "using GeometryBasics\n", 527 | "using MeshCat\n", 528 | "using Rotations\n", 529 | "using StaticArrays\n", 530 | "\n", 531 | "# Kinematics\n", 532 | "kinematics(q) = [q[3], q[4]]\n", 533 | "\n", 534 | "# Visualization\n", 535 | "function visualize!(vis, q, Δt)\n", 536 | "\n", 537 | " # body radius\n", 538 | " r_body = 0.2\n", 539 | "\t# foot radius\n", 540 | " r_foot = 0.1\n", 541 | "\n", 542 | "\t# leg width\n", 543 | " r_leg = 0.5 * r_foot\n", 544 | "\n", 545 | "\t# set background to white\n", 546 | "\tsetvisible!(vis[\"/Background\"], true)\n", 547 | " setprop!(vis[\"/Background\"], \"top_color\", RGBA(1.0, 1.0, 1.0, 1.0))\n", 548 | " setprop!(vis[\"/Background\"], \"bottom_color\", RGBA(1.0, 1.0, 1.0, 1.0))\n", 549 | " setvisible!(vis[\"/Axes\"], false)\n", 550 | "\n", 551 | "\t# create body\n", 552 | " setobject!(vis[\"body\"], Sphere(Point3f0(0),\n", 553 | " convert(Float32, r_body)),\n", 554 | " MeshPhongMaterial(color = RGBA(0, 1, 0, 1.0)))\n", 555 | "\n", 556 | "\t# create foot\n", 557 | " setobject!(vis[\"foot\"], Sphere(Point3f0(0),\n", 558 | " convert(Float32, r_foot)),\n", 559 | " MeshPhongMaterial(color = RGBA(1.0, 165.0 / 255.0, 0, 1.0)))\n", 560 | "\n", 561 | "# \t# create leg\n", 562 | " n_leg = 100\n", 563 | " for i = 1:n_leg\n", 564 | " setobject!(vis[\"leg$i\"], Sphere(Point3f0(0),\n", 565 | " convert(Float32, r_leg)),\n", 566 | " MeshPhongMaterial(color = RGBA(0, 0, 0, 1.0)))\n", 567 | " end\n", 568 | "\n", 569 | " p_leg = [zeros(3) for i = 1:n_leg]\n", 570 | "\n", 571 | "\t# animation\n", 572 | " anim = MeshCat.Animation(convert(Int, floor(1.0 / Δt)))\n", 573 | "\n", 574 | " for t = 1:length(q)\n", 575 | " p_body = [q[t][1], 0.0, q[t][2]]\n", 576 | " p_foot = [kinematics(q[t])[1], 0.0, kinematics(q[t])[2]]\n", 577 | "\n", 578 | " dir = [q[t][3] - q[t][1]; q[t][4] - q[t][2]]\n", 579 | " dir = dir ./ norm(dir)\n", 580 | " 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", 581 | " for i = 1:n_leg\n", 582 | " p_leg[i] = [q[t][1] + r_range[i] * dir[1], 0.0, q[t][2] + r_range[i] * dir[2]]\n", 583 | " end\n", 584 | " \n", 585 | " z_shift = [0.0; 0.0; r_foot]\n", 586 | "\n", 587 | " MeshCat.atframe(anim, t) do\n", 588 | " settransform!(vis[\"body\"], Translation(p_body + z_shift))\n", 589 | " settransform!(vis[\"foot\"], Translation(p_foot + z_shift))\n", 590 | "\n", 591 | " for i = 1:n_leg\n", 592 | " settransform!(vis[\"leg$i\"], Translation(p_leg[i] + z_shift))\n", 593 | " end\n", 594 | " end\n", 595 | " end\n", 596 | "\n", 597 | " MeshCat.setanimation!(vis, anim)\n", 598 | "end\n", 599 | "\n", 600 | "vis = Visualizer()\n", 601 | "render(vis)" 602 | ] 603 | }, 604 | { 605 | "cell_type": "code", 606 | "execution_count": null, 607 | "metadata": {}, 608 | "outputs": [], 609 | "source": [ 610 | "q = [SVector{4}(x[1:4]) for x in eachcol(xtraj)];\n", 611 | "visualize!(vis, q, h)" 612 | ] 613 | }, 614 | { 615 | "cell_type": "code", 616 | "execution_count": null, 617 | "metadata": {}, 618 | "outputs": [], 619 | "source": [] 620 | } 621 | ], 622 | "metadata": { 623 | "kernelspec": { 624 | "display_name": "Julia 1.6.7", 625 | "language": "julia", 626 | "name": "julia-1.6" 627 | }, 628 | "language_info": { 629 | "file_extension": ".jl", 630 | "mimetype": "application/julia", 631 | "name": "julia", 632 | "version": "1.6.7" 633 | } 634 | }, 635 | "nbformat": 4, 636 | "nbformat_minor": 4 637 | } 638 | -------------------------------------------------------------------------------- /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.7", 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.7" 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-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 17/Lecture 17.pdf -------------------------------------------------------------------------------- /Lecture 18/Lecture 18.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 18/Lecture 18.pdf -------------------------------------------------------------------------------- /Lecture 18/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | [[Adapt]] 4 | deps = ["LinearAlgebra", "Requires"] 5 | git-tree-sha1 = "cc37d689f599e8df4f464b2fa3870ff7db7492ef" 6 | uuid = "79e6a3ab-5dfb-504d-930d-738a2a938a0e" 7 | version = "3.6.1" 8 | 9 | [[Altro]] 10 | deps = ["BenchmarkTools", "Crayons", "FiniteDiff", "Formatting", "ForwardDiff", "Interpolations", "LinearAlgebra", "Logging", "Octavian", "QDLDL_jll", "RobotDynamics", "RobotZoo", "Rotations", "SolverLogging", "SparseArrays", "StaticArrays", "Statistics", "TimerOutputs", "TrajectoryOptimization"] 11 | git-tree-sha1 = "e6f0060e3d15ba797a303a9f63705e3871238a13" 12 | uuid = "5dcf52e5-e2fb-48e0-b826-96f46d2e3e73" 13 | version = "0.5.0" 14 | 15 | [[ArgTools]] 16 | uuid = "0dad84c5-d112-42e6-8d28-ef12dabb789f" 17 | 18 | [[ArrayInterface]] 19 | deps = ["Adapt", "LinearAlgebra", "Requires", "SnoopPrecompile", "SparseArrays", "SuiteSparse"] 20 | git-tree-sha1 = "d76e35eb01ce7264ab8ee18ca9ba3fb4c844b6df" 21 | uuid = "4fba245c-0d91-5ea0-9b3e-6abc04ee57a9" 22 | version = "7.4.1" 23 | 24 | [[ArrayInterfaceCore]] 25 | deps = ["LinearAlgebra", "SnoopPrecompile", "SparseArrays", "SuiteSparse"] 26 | git-tree-sha1 = "e5f08b5689b1aad068e01751889f2f615c7db36d" 27 | uuid = "30b0a656-2188-435a-8636-2ec0e6a096e2" 28 | version = "0.1.29" 29 | 30 | [[Artifacts]] 31 | uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33" 32 | 33 | [[AxisAlgorithms]] 34 | deps = ["LinearAlgebra", "Random", "SparseArrays", "WoodburyMatrices"] 35 | git-tree-sha1 = "66771c8d21c8ff5e3a93379480a2307ac36863f7" 36 | uuid = "13072b0f-2c55-5437-9ae7-d433b7a33950" 37 | version = "1.0.1" 38 | 39 | [[Base64]] 40 | uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" 41 | 42 | [[BenchmarkTools]] 43 | deps = ["JSON", "Logging", "Printf", "Profile", "Statistics", "UUIDs"] 44 | git-tree-sha1 = "d9a9701b899b30332bbcb3e1679c41cce81fb0e8" 45 | uuid = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf" 46 | version = "1.3.2" 47 | 48 | [[BitTwiddlingConvenienceFunctions]] 49 | deps = ["Static"] 50 | git-tree-sha1 = "0c5f81f47bbbcf4aea7b2959135713459170798b" 51 | uuid = "62783981-4cbd-42fc-bca8-16325de8dc4b" 52 | version = "0.1.5" 53 | 54 | [[Bzip2_jll]] 55 | deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"] 56 | git-tree-sha1 = "19a35467a82e236ff51bc17a3a44b69ef35185a2" 57 | uuid = "6e34b625-4abd-537c-b88f-471c36dfa7a0" 58 | version = "1.0.8+0" 59 | 60 | [[CPUSummary]] 61 | deps = ["CpuId", "IfElse", "Static"] 62 | git-tree-sha1 = "2c144ddb46b552f72d7eafe7cc2f50746e41ea21" 63 | uuid = "2a0fbf3d-bb9c-48f3-b0a9-814d99fd7ab9" 64 | version = "0.2.2" 65 | 66 | [[ChainRulesCore]] 67 | deps = ["Compat", "LinearAlgebra", "SparseArrays"] 68 | git-tree-sha1 = "c6d890a52d2c4d55d326439580c3b8d0875a77d9" 69 | uuid = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" 70 | version = "1.15.7" 71 | 72 | [[ChangesOfVariables]] 73 | deps = ["ChainRulesCore", "LinearAlgebra", "Test"] 74 | git-tree-sha1 = "485193efd2176b88e6622a39a246f8c5b600e74e" 75 | uuid = "9e997f8a-9a97-42d5-a9f1-ce6bfc15e2c0" 76 | version = "0.1.6" 77 | 78 | [[CloseOpenIntervals]] 79 | deps = ["Static", "StaticArrayInterface"] 80 | git-tree-sha1 = "70232f82ffaab9dc52585e0dd043b5e0c6b714f1" 81 | uuid = "fb6a15b2-703c-40df-9091-08a04967cfa9" 82 | version = "0.1.12" 83 | 84 | [[CodecBzip2]] 85 | deps = ["Bzip2_jll", "Libdl", "TranscodingStreams"] 86 | git-tree-sha1 = "2e62a725210ce3c3c2e1a3080190e7ca491f18d7" 87 | uuid = "523fee87-0ab8-5b00-afb7-3ecf72e48cfd" 88 | version = "0.7.2" 89 | 90 | [[CodecZlib]] 91 | deps = ["TranscodingStreams", "Zlib_jll"] 92 | git-tree-sha1 = "9c209fb7536406834aa938fb149964b985de6c83" 93 | uuid = "944b1d66-785c-5afd-91f1-9de20f533193" 94 | version = "0.7.1" 95 | 96 | [[ColorTypes]] 97 | deps = ["FixedPointNumbers", "Random"] 98 | git-tree-sha1 = "eb7f0f8307f71fac7c606984ea5fb2817275d6e4" 99 | uuid = "3da002f7-5984-5a60-b8a6-cbb66c0b333f" 100 | version = "0.11.4" 101 | 102 | [[Colors]] 103 | deps = ["ColorTypes", "FixedPointNumbers", "Reexport"] 104 | git-tree-sha1 = "fc08e5930ee9a4e03f84bfb5211cb54e7769758a" 105 | uuid = "5ae59095-9a9b-59fe-a467-6f913c188581" 106 | version = "0.12.10" 107 | 108 | [[CommonSubexpressions]] 109 | deps = ["MacroTools", "Test"] 110 | git-tree-sha1 = "7b8a93dba8af7e3b42fecabf646260105ac373f7" 111 | uuid = "bbf7d656-a473-5ed7-a52c-81e309532950" 112 | version = "0.3.0" 113 | 114 | [[Compat]] 115 | deps = ["Dates", "LinearAlgebra", "UUIDs"] 116 | git-tree-sha1 = "7a60c856b9fa189eb34f5f8a6f6b5529b7942957" 117 | uuid = "34da2185-b29b-5c13-b0c7-acf172513d20" 118 | version = "4.6.1" 119 | 120 | [[CompilerSupportLibraries_jll]] 121 | deps = ["Artifacts", "Libdl"] 122 | uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" 123 | 124 | [[Conda]] 125 | deps = ["Downloads", "JSON", "VersionParsing"] 126 | git-tree-sha1 = "e32a90da027ca45d84678b826fffd3110bb3fc90" 127 | uuid = "8f4d0f93-b110-5947-807f-2305c1781a2d" 128 | version = "1.8.0" 129 | 130 | [[ConstructionBase]] 131 | deps = ["LinearAlgebra"] 132 | git-tree-sha1 = "89a9db8d28102b094992472d333674bd1a83ce2a" 133 | uuid = "187b0558-2788-49d3-abe0-74a17ed4e7c9" 134 | version = "1.5.1" 135 | 136 | [[CpuId]] 137 | deps = ["Markdown"] 138 | git-tree-sha1 = "32d125af0fb8ec3f8935896122c5e345709909e5" 139 | uuid = "adafc99b-e345-5852-983c-f28acb93d879" 140 | version = "0.3.0" 141 | 142 | [[Crayons]] 143 | git-tree-sha1 = "249fe38abf76d48563e2f4556bebd215aa317e15" 144 | uuid = "a8cc5b0e-0ffa-5ad4-8c14-923d3ee1735f" 145 | version = "4.1.1" 146 | 147 | [[DataStructures]] 148 | deps = ["Compat", "InteractiveUtils", "OrderedCollections"] 149 | git-tree-sha1 = "d1fff3a548102f48987a52a2e0d114fa97d730f0" 150 | uuid = "864edb3b-99cc-5e75-8d2d-829cb0a9cfe8" 151 | version = "0.18.13" 152 | 153 | [[Dates]] 154 | deps = ["Printf"] 155 | uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" 156 | 157 | [[DiffResults]] 158 | deps = ["StaticArraysCore"] 159 | git-tree-sha1 = "782dd5f4561f5d267313f23853baaaa4c52ea621" 160 | uuid = "163ba53b-c6d8-5494-b064-1a9d43ac40c5" 161 | version = "1.1.0" 162 | 163 | [[DiffRules]] 164 | deps = ["IrrationalConstants", "LogExpFunctions", "NaNMath", "Random", "SpecialFunctions"] 165 | git-tree-sha1 = "a4ad7ef19d2cdc2eff57abbbe68032b1cd0bd8f8" 166 | uuid = "b552c78f-8df3-52c6-915a-8e097449b14b" 167 | version = "1.13.0" 168 | 169 | [[Distributed]] 170 | deps = ["Random", "Serialization", "Sockets"] 171 | uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b" 172 | 173 | [[DocStringExtensions]] 174 | deps = ["LibGit2"] 175 | git-tree-sha1 = "b19534d1895d702889b219c382a6e18010797f0b" 176 | uuid = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" 177 | version = "0.8.6" 178 | 179 | [[Downloads]] 180 | deps = ["ArgTools", "LibCURL", "NetworkOptions"] 181 | uuid = "f43a241f-c20a-4ad4-852c-f6b1247861c6" 182 | 183 | [[ExprTools]] 184 | git-tree-sha1 = "c1d06d129da9f55715c6c212866f5b1bddc5fa00" 185 | uuid = "e2ba6199-217a-4e67-a87a-7c52f15ade04" 186 | version = "0.1.9" 187 | 188 | [[FiniteDiff]] 189 | deps = ["ArrayInterface", "LinearAlgebra", "Requires", "Setfield", "SparseArrays", "StaticArrays"] 190 | git-tree-sha1 = "03fcb1c42ec905d15b305359603888ec3e65f886" 191 | uuid = "6a86dc24-6348-571c-b903-95158fe2bd41" 192 | version = "2.19.0" 193 | 194 | [[FixedPointNumbers]] 195 | deps = ["Statistics"] 196 | git-tree-sha1 = "335bfdceacc84c5cdf16aadc768aa5ddfc5383cc" 197 | uuid = "53c48c17-4a7d-5ca2-90c5-79b7896eea93" 198 | version = "0.8.4" 199 | 200 | [[Formatting]] 201 | deps = ["Printf"] 202 | git-tree-sha1 = "8339d61043228fdd3eb658d86c926cb282ae72a8" 203 | uuid = "59287772-0a20-5a39-b81b-1366585eb4c0" 204 | version = "0.4.2" 205 | 206 | [[ForwardDiff]] 207 | deps = ["CommonSubexpressions", "DiffResults", "DiffRules", "LinearAlgebra", "LogExpFunctions", "NaNMath", "Preferences", "Printf", "Random", "SpecialFunctions", "StaticArrays"] 208 | git-tree-sha1 = "00e252f4d706b3d55a8863432e742bf5717b498d" 209 | uuid = "f6369f11-7733-5829-9624-2563aa707210" 210 | version = "0.10.35" 211 | 212 | [[Future]] 213 | deps = ["Random"] 214 | uuid = "9fa8497b-333b-5362-9e8d-4d0656e87820" 215 | 216 | [[HostCPUFeatures]] 217 | deps = ["BitTwiddlingConvenienceFunctions", "IfElse", "Libdl", "Static"] 218 | git-tree-sha1 = "734fd90dd2f920a2f1921d5388dcebe805b262dc" 219 | uuid = "3e5b6fbb-0976-4d2c-9146-d79de83f2fb0" 220 | version = "0.1.14" 221 | 222 | [[IfElse]] 223 | git-tree-sha1 = "debdd00ffef04665ccbb3e150747a77560e8fad1" 224 | uuid = "615f187c-cbe4-4ef1-ba3b-2fcf58d6d173" 225 | version = "0.1.1" 226 | 227 | [[InteractiveUtils]] 228 | deps = ["Markdown"] 229 | uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" 230 | 231 | [[Interpolations]] 232 | deps = ["AxisAlgorithms", "ChainRulesCore", "LinearAlgebra", "OffsetArrays", "Random", "Ratios", "Requires", "SharedArrays", "SparseArrays", "StaticArrays", "WoodburyMatrices"] 233 | git-tree-sha1 = "b7bc05649af456efc75d178846f47006c2c4c3c7" 234 | uuid = "a98d9a8b-a2ab-59e6-89dd-64a1c18fca59" 235 | version = "0.13.6" 236 | 237 | [[InverseFunctions]] 238 | deps = ["Test"] 239 | git-tree-sha1 = "49510dfcb407e572524ba94aeae2fced1f3feb0f" 240 | uuid = "3587e190-3f89-42d0-90ee-14403ec27112" 241 | version = "0.1.8" 242 | 243 | [[IrrationalConstants]] 244 | git-tree-sha1 = "630b497eafcc20001bba38a4651b327dcfc491d2" 245 | uuid = "92d709cd-6900-40b7-9082-c6be49f344b6" 246 | version = "0.2.2" 247 | 248 | [[JLLWrappers]] 249 | deps = ["Preferences"] 250 | git-tree-sha1 = "abc9885a7ca2052a736a600f7fa66209f96506e1" 251 | uuid = "692b3bcd-3c85-4b1f-b108-f13ce0eb3210" 252 | version = "1.4.1" 253 | 254 | [[JSON]] 255 | deps = ["Dates", "Mmap", "Parsers", "Unicode"] 256 | git-tree-sha1 = "3c837543ddb02250ef42f4738347454f95079d4e" 257 | uuid = "682c06a0-de6a-54ab-a142-c8b1cf79cde6" 258 | version = "0.21.3" 259 | 260 | [[LaTeXStrings]] 261 | git-tree-sha1 = "f2355693d6778a178ade15952b7ac47a4ff97996" 262 | uuid = "b964fa9f-0449-5b57-a5c2-d3ea65f4040f" 263 | version = "1.3.0" 264 | 265 | [[LayoutPointers]] 266 | deps = ["ArrayInterface", "LinearAlgebra", "ManualMemory", "SIMDTypes", "Static", "StaticArrayInterface"] 267 | git-tree-sha1 = "88b8f66b604da079a627b6fb2860d3704a6729a1" 268 | uuid = "10f19ff3-798f-405d-979b-55457f8fc047" 269 | version = "0.1.14" 270 | 271 | [[LibCURL]] 272 | deps = ["LibCURL_jll", "MozillaCACerts_jll"] 273 | uuid = "b27032c2-a3e7-50c8-80cd-2d36dbcbfd21" 274 | 275 | [[LibCURL_jll]] 276 | deps = ["Artifacts", "LibSSH2_jll", "Libdl", "MbedTLS_jll", "Zlib_jll", "nghttp2_jll"] 277 | uuid = "deac9b47-8bc7-5906-a0fe-35ac56dc84c0" 278 | 279 | [[LibGit2]] 280 | deps = ["Base64", "NetworkOptions", "Printf", "SHA"] 281 | uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" 282 | 283 | [[LibSSH2_jll]] 284 | deps = ["Artifacts", "Libdl", "MbedTLS_jll"] 285 | uuid = "29816b5a-b9ab-546f-933c-edad1886dfa8" 286 | 287 | [[Libdl]] 288 | uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" 289 | 290 | [[LinearAlgebra]] 291 | deps = ["Libdl"] 292 | uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 293 | 294 | [[LogExpFunctions]] 295 | deps = ["ChainRulesCore", "ChangesOfVariables", "DocStringExtensions", "InverseFunctions", "IrrationalConstants", "LinearAlgebra"] 296 | git-tree-sha1 = "0a1b7c2863e44523180fdb3146534e265a91870b" 297 | uuid = "2ab3a3ac-af41-5b50-aa03-7779005ae688" 298 | version = "0.3.23" 299 | 300 | [[Logging]] 301 | uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" 302 | 303 | [[LoopVectorization]] 304 | deps = ["ArrayInterface", "ArrayInterfaceCore", "CPUSummary", "ChainRulesCore", "CloseOpenIntervals", "DocStringExtensions", "ForwardDiff", "HostCPUFeatures", "IfElse", "LayoutPointers", "LinearAlgebra", "OffsetArrays", "PolyesterWeave", "SIMDTypes", "SLEEFPirates", "SnoopPrecompile", "SpecialFunctions", "Static", "StaticArrayInterface", "ThreadingUtilities", "UnPack", "VectorizationBase"] 305 | git-tree-sha1 = "a282dbdbc2860134d6809acd951543ce359bcf15" 306 | uuid = "bdcacae8-1622-11e9-2a5c-532679323890" 307 | version = "0.12.155" 308 | 309 | [[MacroTools]] 310 | deps = ["Markdown", "Random"] 311 | git-tree-sha1 = "42324d08725e200c23d4dfb549e0d5d89dede2d2" 312 | uuid = "1914dd2f-81c6-5fcd-8719-6d5c9610ff09" 313 | version = "0.5.10" 314 | 315 | [[ManualMemory]] 316 | git-tree-sha1 = "bcaef4fc7a0cfe2cba636d84cda54b5e4e4ca3cd" 317 | uuid = "d125e4d3-2237-4719-b19c-fa641b8a4667" 318 | version = "0.1.8" 319 | 320 | [[Markdown]] 321 | deps = ["Base64"] 322 | uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" 323 | 324 | [[MathOptInterface]] 325 | deps = ["BenchmarkTools", "CodecBzip2", "CodecZlib", "DataStructures", "ForwardDiff", "JSON", "LinearAlgebra", "MutableArithmetics", "NaNMath", "OrderedCollections", "Printf", "SnoopPrecompile", "SparseArrays", "SpecialFunctions", "Test", "Unicode"] 326 | git-tree-sha1 = "3ba708c18f4a5ee83f3a6fb67a2775147a1f59f5" 327 | uuid = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" 328 | version = "1.13.2" 329 | 330 | [[MbedTLS_jll]] 331 | deps = ["Artifacts", "Libdl"] 332 | uuid = "c8ffd9c3-330d-5841-b78e-0817d7145fa1" 333 | 334 | [[Mmap]] 335 | uuid = "a63ad114-7e13-5084-954f-fe012c677804" 336 | 337 | [[MozillaCACerts_jll]] 338 | uuid = "14a3606d-f60d-562e-9121-12d972cd8159" 339 | 340 | [[MutableArithmetics]] 341 | deps = ["LinearAlgebra", "SparseArrays", "Test"] 342 | git-tree-sha1 = "3295d296288ab1a0a2528feb424b854418acff57" 343 | uuid = "d8a4904e-b15c-11e9-3269-09a3773c0cb0" 344 | version = "1.2.3" 345 | 346 | [[NaNMath]] 347 | deps = ["OpenLibm_jll"] 348 | git-tree-sha1 = "0877504529a3e5c3343c6f8b4c0381e57e4387e4" 349 | uuid = "77ba4419-2d1f-58cd-9bb1-8ffee604a2e3" 350 | version = "1.0.2" 351 | 352 | [[NetworkOptions]] 353 | uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908" 354 | 355 | [[OSQP]] 356 | deps = ["Libdl", "LinearAlgebra", "MathOptInterface", "OSQP_jll", "SparseArrays"] 357 | git-tree-sha1 = "3514d0aff03027a9c1b0b312151619c9feec412a" 358 | uuid = "ab2f91bb-94b4-55e3-9ba0-7f65df51de79" 359 | version = "0.8.0" 360 | 361 | [[OSQP_jll]] 362 | deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"] 363 | git-tree-sha1 = "d0f73698c33e04e557980a06d75c2d82e3f0eb49" 364 | uuid = "9c4f68bf-6205-5545-a508-2878b064d984" 365 | version = "0.600.200+0" 366 | 367 | [[Octavian]] 368 | deps = ["CPUSummary", "IfElse", "LoopVectorization", "ManualMemory", "PolyesterWeave", "Requires", "SnoopPrecompile", "Static", "StaticArrayInterface", "ThreadingUtilities", "VectorizationBase"] 369 | git-tree-sha1 = "f73041af8b18e6e098a93eeffb135db324d1ebf4" 370 | uuid = "6fd5a793-0b7e-452c-907f-f8bfe9c57db4" 371 | version = "0.3.21" 372 | 373 | [[OffsetArrays]] 374 | deps = ["Adapt"] 375 | git-tree-sha1 = "82d7c9e310fe55aa54996e6f7f94674e2a38fcb4" 376 | uuid = "6fe1bfb0-de20-5000-8ca7-80f57d26f881" 377 | version = "1.12.9" 378 | 379 | [[OpenLibm_jll]] 380 | deps = ["Artifacts", "Libdl"] 381 | uuid = "05823500-19ac-5b8b-9628-191a04bc5112" 382 | 383 | [[OpenSpecFun_jll]] 384 | deps = ["Artifacts", "CompilerSupportLibraries_jll", "JLLWrappers", "Libdl", "Pkg"] 385 | git-tree-sha1 = "13652491f6856acfd2db29360e1bbcd4565d04f1" 386 | uuid = "efe28fd5-8261-553b-a9e1-b2916fc3738e" 387 | version = "0.5.5+0" 388 | 389 | [[OrderedCollections]] 390 | git-tree-sha1 = "d78db6df34313deaca15c5c0b9ff562c704fe1ab" 391 | uuid = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" 392 | version = "1.5.0" 393 | 394 | [[Parsers]] 395 | deps = ["Dates", "SnoopPrecompile"] 396 | git-tree-sha1 = "478ac6c952fddd4399e71d4779797c538d0ff2bf" 397 | uuid = "69de0a69-1ddd-5017-9359-2bf0b02dc9f0" 398 | version = "2.5.8" 399 | 400 | [[Pkg]] 401 | deps = ["Artifacts", "Dates", "Downloads", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "Serialization", "TOML", "Tar", "UUIDs", "p7zip_jll"] 402 | uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" 403 | 404 | [[PolyesterWeave]] 405 | deps = ["BitTwiddlingConvenienceFunctions", "CPUSummary", "IfElse", "Static", "ThreadingUtilities"] 406 | git-tree-sha1 = "240d7170f5ffdb285f9427b92333c3463bf65bf6" 407 | uuid = "1d0040c9-8b98-4ee7-8388-3f51789ca0ad" 408 | version = "0.2.1" 409 | 410 | [[Preferences]] 411 | deps = ["TOML"] 412 | git-tree-sha1 = "47e5f437cc0e7ef2ce8406ce1e7e24d44915f88d" 413 | uuid = "21216c6a-2e73-6563-6e65-726566657250" 414 | version = "1.3.0" 415 | 416 | [[Printf]] 417 | deps = ["Unicode"] 418 | uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" 419 | 420 | [[Profile]] 421 | deps = ["Printf"] 422 | uuid = "9abbd945-dff8-562f-b5e8-e1ebf5ef1b79" 423 | 424 | [[PyCall]] 425 | deps = ["Conda", "Dates", "Libdl", "LinearAlgebra", "MacroTools", "Serialization", "VersionParsing"] 426 | git-tree-sha1 = "62f417f6ad727987c755549e9cd88c46578da562" 427 | uuid = "438e738f-606a-5dbb-bf0a-cddfbfd45ab0" 428 | version = "1.95.1" 429 | 430 | [[PyPlot]] 431 | deps = ["Colors", "LaTeXStrings", "PyCall", "Sockets", "Test", "VersionParsing"] 432 | git-tree-sha1 = "92e7ca803b579b8b817f004e74b205a706d9a974" 433 | uuid = "d330b81b-6aea-500a-939a-2ce795aea3ee" 434 | version = "2.11.1" 435 | 436 | [[QDLDL_jll]] 437 | deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"] 438 | git-tree-sha1 = "54cb0359f294f8b3cbf4ba30208e41d7b8bd116b" 439 | uuid = "9ae34a36-e1b6-54e9-a33d-8bba9913f854" 440 | version = "0.1.5+0" 441 | 442 | [[REPL]] 443 | deps = ["InteractiveUtils", "Markdown", "Sockets", "Unicode"] 444 | uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" 445 | 446 | [[Random]] 447 | deps = ["Serialization"] 448 | uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" 449 | 450 | [[Ratios]] 451 | deps = ["Requires"] 452 | git-tree-sha1 = "dc84268fe0e3335a62e315a3a7cf2afa7178a734" 453 | uuid = "c84ed2f1-dad5-54f0-aa8e-dbefe2724439" 454 | version = "0.4.3" 455 | 456 | [[RecipesBase]] 457 | deps = ["SnoopPrecompile"] 458 | git-tree-sha1 = "261dddd3b862bd2c940cf6ca4d1c8fe593e457c8" 459 | uuid = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" 460 | version = "1.3.3" 461 | 462 | [[Reexport]] 463 | git-tree-sha1 = "45e428421666073eab6f2da5c9d310d99bb12f9b" 464 | uuid = "189a3867-3050-52da-a836-e630ba90ab69" 465 | version = "1.2.2" 466 | 467 | [[Requires]] 468 | deps = ["UUIDs"] 469 | git-tree-sha1 = "838a3a4188e2ded87a4f9f184b4b0d78a1e91cb7" 470 | uuid = "ae029012-a4dd-5104-9daa-d747884805df" 471 | version = "1.3.0" 472 | 473 | [[RobotDynamics]] 474 | deps = ["FiniteDiff", "ForwardDiff", "LinearAlgebra", "Pkg", "RecipesBase", "Rotations", "SparseArrays", "StaticArrays"] 475 | git-tree-sha1 = "45a2cc0d9ee87c1f04486b5a7043262b2c272ae8" 476 | uuid = "38ceca67-d8d3-44e8-9852-78a5596522e1" 477 | version = "0.4.7" 478 | 479 | [[RobotZoo]] 480 | deps = ["FiniteDiff", "ForwardDiff", "LinearAlgebra", "RobotDynamics", "Rotations", "StaticArrays"] 481 | git-tree-sha1 = "e0b5ddc4451c38ab470ac3e5089939824beb89fb" 482 | uuid = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 483 | version = "0.3.1" 484 | 485 | [[Rotations]] 486 | deps = ["LinearAlgebra", "Random", "StaticArrays", "Statistics"] 487 | git-tree-sha1 = "6a23472b6b097d66da87785b61137142ac104f94" 488 | uuid = "6038ab10-8711-5258-84ad-4b1120ba62dc" 489 | version = "1.0.4" 490 | 491 | [[SHA]] 492 | uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" 493 | 494 | [[SIMDTypes]] 495 | git-tree-sha1 = "330289636fb8107c5f32088d2741e9fd7a061a5c" 496 | uuid = "94e857df-77ce-4151-89e5-788b33177be4" 497 | version = "0.1.0" 498 | 499 | [[SLEEFPirates]] 500 | deps = ["IfElse", "Static", "VectorizationBase"] 501 | git-tree-sha1 = "cda0aece8080e992f6370491b08ef3909d1c04e7" 502 | uuid = "476501e8-09a2-5ece-8869-fb82de89a1fa" 503 | version = "0.6.38" 504 | 505 | [[Serialization]] 506 | uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" 507 | 508 | [[Setfield]] 509 | deps = ["ConstructionBase", "Future", "MacroTools", "StaticArraysCore"] 510 | git-tree-sha1 = "e2cc6d8c88613c05e1defb55170bf5ff211fbeac" 511 | uuid = "efcf1570-3423-57d1-acb7-fd33fddbac46" 512 | version = "1.1.1" 513 | 514 | [[SharedArrays]] 515 | deps = ["Distributed", "Mmap", "Random", "Serialization"] 516 | uuid = "1a1011a3-84de-559e-8e89-a11a2f7dc383" 517 | 518 | [[SnoopPrecompile]] 519 | deps = ["Preferences"] 520 | git-tree-sha1 = "e760a70afdcd461cf01a575947738d359234665c" 521 | uuid = "66db9d55-30c0-4569-8b51-7e840670fc0c" 522 | version = "1.0.3" 523 | 524 | [[Sockets]] 525 | uuid = "6462fe0b-24de-5631-8697-dd941f90decc" 526 | 527 | [[SolverLogging]] 528 | deps = ["Crayons", "Formatting", "Logging", "Printf"] 529 | git-tree-sha1 = "ebe090fc0a0f83685b5bfd7d1eb3fd870bbd1773" 530 | uuid = "c2e08473-88be-4f39-9d3c-afcdb6e3aeb8" 531 | version = "0.2.0" 532 | 533 | [[SparseArrays]] 534 | deps = ["LinearAlgebra", "Random"] 535 | uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" 536 | 537 | [[SpecialFunctions]] 538 | deps = ["ChainRulesCore", "IrrationalConstants", "LogExpFunctions", "OpenLibm_jll", "OpenSpecFun_jll"] 539 | git-tree-sha1 = "ef28127915f4229c971eb43f3fc075dd3fe91880" 540 | uuid = "276daf66-3868-5448-9aa4-cd146d93841b" 541 | version = "2.2.0" 542 | 543 | [[Static]] 544 | deps = ["IfElse"] 545 | git-tree-sha1 = "08be5ee09a7632c32695d954a602df96a877bf0d" 546 | uuid = "aedffcd0-7271-4cad-89d0-dc628f76c6d3" 547 | version = "0.8.6" 548 | 549 | [[StaticArrayInterface]] 550 | deps = ["ArrayInterface", "Compat", "IfElse", "LinearAlgebra", "Requires", "SnoopPrecompile", "SparseArrays", "Static", "SuiteSparse"] 551 | git-tree-sha1 = "fd5f417fd7e103c121b0a0b4a6902f03991111f4" 552 | uuid = "0d7ed370-da01-4f52-bd93-41d350b8b718" 553 | version = "1.3.0" 554 | 555 | [[StaticArrays]] 556 | deps = ["LinearAlgebra", "Random", "StaticArraysCore", "Statistics"] 557 | git-tree-sha1 = "b8d897fe7fa688e93aef573711cb207c08c9e11e" 558 | uuid = "90137ffa-7385-5640-81b9-e52037218182" 559 | version = "1.5.19" 560 | 561 | [[StaticArraysCore]] 562 | git-tree-sha1 = "6b7ba252635a5eff6a0b0664a41ee140a1c9e72a" 563 | uuid = "1e83bf80-4336-4d27-bf5d-d5a4f845583c" 564 | version = "1.4.0" 565 | 566 | [[Statistics]] 567 | deps = ["LinearAlgebra", "SparseArrays"] 568 | uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" 569 | 570 | [[SuiteSparse]] 571 | deps = ["Libdl", "LinearAlgebra", "Serialization", "SparseArrays"] 572 | uuid = "4607b0f0-06f3-5cda-b6b1-a6196a1729e9" 573 | 574 | [[TOML]] 575 | deps = ["Dates"] 576 | uuid = "fa267f1f-6049-4f14-aa54-33bafae1ed76" 577 | 578 | [[Tar]] 579 | deps = ["ArgTools", "SHA"] 580 | uuid = "a4e569a6-e804-4fa4-b0f3-eef7a1d5b13e" 581 | 582 | [[Test]] 583 | deps = ["InteractiveUtils", "Logging", "Random", "Serialization"] 584 | uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40" 585 | 586 | [[ThreadingUtilities]] 587 | deps = ["ManualMemory"] 588 | git-tree-sha1 = "c97f60dd4f2331e1a495527f80d242501d2f9865" 589 | uuid = "8290d209-cae3-49c0-8002-c8c24d57dab5" 590 | version = "0.5.1" 591 | 592 | [[TimerOutputs]] 593 | deps = ["ExprTools", "Printf"] 594 | git-tree-sha1 = "f2fd3f288dfc6f507b0c3a2eb3bac009251e548b" 595 | uuid = "a759f4b9-e2f1-59dc-863e-4aeb61b1ea8f" 596 | version = "0.5.22" 597 | 598 | [[TrajectoryOptimization]] 599 | deps = ["DocStringExtensions", "FiniteDiff", "ForwardDiff", "LinearAlgebra", "RobotDynamics", "RobotZoo", "Rotations", "SparseArrays", "StaticArrays"] 600 | git-tree-sha1 = "1e449355d1a81f2bb54f7f5aba8ac33b69599130" 601 | uuid = "c79d492b-0548-5874-b488-5a62c1d9d0ca" 602 | version = "0.7.0" 603 | 604 | [[TranscodingStreams]] 605 | deps = ["Random", "Test"] 606 | git-tree-sha1 = "94f38103c984f89cf77c402f2a68dbd870f8165f" 607 | uuid = "3bb67fe8-82b1-5028-8e26-92a6c54297fa" 608 | version = "0.9.11" 609 | 610 | [[UUIDs]] 611 | deps = ["Random", "SHA"] 612 | uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" 613 | 614 | [[UnPack]] 615 | git-tree-sha1 = "387c1f73762231e86e0c9c5443ce3b4a0a9a0c2b" 616 | uuid = "3a884ed6-31ef-47d7-9d2a-63182c4928ed" 617 | version = "1.0.2" 618 | 619 | [[Unicode]] 620 | uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" 621 | 622 | [[VectorizationBase]] 623 | deps = ["ArrayInterface", "CPUSummary", "HostCPUFeatures", "IfElse", "LayoutPointers", "Libdl", "LinearAlgebra", "SIMDTypes", "Static", "StaticArrayInterface"] 624 | git-tree-sha1 = "f78fc4c3abbf5ab65108a41664e88fb131ab8946" 625 | uuid = "3d5dd08c-fd9d-11e8-17fa-ed2836048c2f" 626 | version = "0.21.63" 627 | 628 | [[VersionParsing]] 629 | git-tree-sha1 = "58d6e80b4ee071f5efd07fda82cb9fbe17200868" 630 | uuid = "81def892-9a0e-5fdd-b105-ffc91e053289" 631 | version = "1.3.0" 632 | 633 | [[WoodburyMatrices]] 634 | deps = ["LinearAlgebra", "SparseArrays"] 635 | git-tree-sha1 = "de67fa59e33ad156a590055375a30b23c40299d3" 636 | uuid = "efce3f68-66dc-5838-9240-27a6d6f5f9b6" 637 | version = "0.5.5" 638 | 639 | [[Zlib_jll]] 640 | deps = ["Libdl"] 641 | uuid = "83775a58-1f1d-513f-b197-d71354ab007a" 642 | 643 | [[nghttp2_jll]] 644 | deps = ["Artifacts", "Libdl"] 645 | uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d" 646 | 647 | [[p7zip_jll]] 648 | deps = ["Artifacts", "Libdl"] 649 | uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0" 650 | -------------------------------------------------------------------------------- /Lecture 18/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | Altro = "5dcf52e5-e2fb-48e0-b826-96f46d2e3e73" 3 | ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" 4 | OSQP = "ab2f91bb-94b4-55e3-9ba0-7f65df51de79" 5 | PyPlot = "d330b81b-6aea-500a-939a-2ce795aea3ee" 6 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 7 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 8 | SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" 9 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 10 | TrajectoryOptimization = "c79d492b-0548-5874-b488-5a62c1d9d0ca" 11 | -------------------------------------------------------------------------------- /Lecture 18/cartpole-ilc.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 TrajectoryOptimization\n", 19 | "using RobotDynamics\n", 20 | "const RD = RobotDynamics\n", 21 | "import RobotZoo.Cartpole\n", 22 | "using StaticArrays\n", 23 | "using SparseArrays\n", 24 | "using LinearAlgebra\n", 25 | "using ForwardDiff\n", 26 | "using PyPlot\n", 27 | "using OSQP" 28 | ] 29 | }, 30 | { 31 | "cell_type": "code", 32 | "execution_count": null, 33 | "metadata": {}, 34 | "outputs": [], 35 | "source": [ 36 | "model = Cartpole()\n", 37 | "n,m = size(model);" 38 | ] 39 | }, 40 | { 41 | "cell_type": "code", 42 | "execution_count": null, 43 | "metadata": {}, 44 | "outputs": [], 45 | "source": [ 46 | "N = 101\n", 47 | "Tf = 5.\n", 48 | "h = Tf/(N-1)" 49 | ] 50 | }, 51 | { 52 | "cell_type": "code", 53 | "execution_count": null, 54 | "metadata": {}, 55 | "outputs": [], 56 | "source": [ 57 | "x0 = @SVector zeros(n)\n", 58 | "xf = @SVector [0, pi, 0, 0]; # i.e. swing up" 59 | ] 60 | }, 61 | { 62 | "cell_type": "code", 63 | "execution_count": null, 64 | "metadata": {}, 65 | "outputs": [], 66 | "source": [ 67 | "# Set up\n", 68 | "Q = 1.0*Diagonal(@SVector ones(n))\n", 69 | "Qf = 100.0*Diagonal(@SVector ones(n))\n", 70 | "R = 0.1*Diagonal(@SVector ones(m))\n", 71 | "obj = LQRObjective(Q,R,Qf,xf,N);" 72 | ] 73 | }, 74 | { 75 | "cell_type": "code", 76 | "execution_count": null, 77 | "metadata": {}, 78 | "outputs": [], 79 | "source": [ 80 | "# Create Empty ConstraintList\n", 81 | "conSet = ConstraintList(n,m,N)\n", 82 | "\n", 83 | "# Control Bounds\n", 84 | "u_bnd = 5.0\n", 85 | "bnd = BoundConstraint(n,m, u_min=-u_bnd, u_max=u_bnd)\n", 86 | "add_constraint!(conSet, bnd, 1:N-1)\n", 87 | "\n", 88 | "# Goal Constraint\n", 89 | "goal = GoalConstraint(xf)\n", 90 | "add_constraint!(conSet, goal, (N-5):N)" 91 | ] 92 | }, 93 | { 94 | "cell_type": "code", 95 | "execution_count": null, 96 | "metadata": {}, 97 | "outputs": [], 98 | "source": [ 99 | "prob = Problem(model, obj, x0, Tf, xf=xf, constraints=conSet);" 100 | ] 101 | }, 102 | { 103 | "cell_type": "code", 104 | "execution_count": null, 105 | "metadata": {}, 106 | "outputs": [], 107 | "source": [ 108 | "u0 = @SVector fill(0.0,m)\n", 109 | "U0 = [u0 for k = 1:N-1]\n", 110 | "initial_controls!(prob, U0)\n", 111 | "rollout!(prob);" 112 | ] 113 | }, 114 | { 115 | "cell_type": "code", 116 | "execution_count": null, 117 | "metadata": {}, 118 | "outputs": [], 119 | "source": [ 120 | "using Altro\n", 121 | "opts = SolverOptions(\n", 122 | " cost_tolerance_intermediate=1e-3,\n", 123 | " penalty_scaling=10.,\n", 124 | " penalty_initial=1.0\n", 125 | ")\n", 126 | "\n", 127 | "altro = ALTROSolver(prob, opts)\n", 128 | "set_options!(altro, show_summary=true)\n", 129 | "solve!(altro);" 130 | ] 131 | }, 132 | { 133 | "cell_type": "code", 134 | "execution_count": null, 135 | "metadata": {}, 136 | "outputs": [], 137 | "source": [ 138 | "# Extract the solution\n", 139 | "Xopt = states(altro);\n", 140 | "Uopt = controls(altro);\n", 141 | "\n", 142 | "xnom = zeros(n,N)\n", 143 | "for k = 1:N\n", 144 | " xnom[:,k] .= Xopt[k]\n", 145 | "end\n", 146 | "\n", 147 | "unom = zeros(m,N-1)\n", 148 | "utraj = zeros(m,N-1)\n", 149 | "for k = 1:N-1\n", 150 | " unom[:,k] .= Uopt[k]\n", 151 | " utraj[:,k] .= Uopt[k]\n", 152 | "end" 153 | ] 154 | }, 155 | { 156 | "cell_type": "code", 157 | "execution_count": null, 158 | "metadata": {}, 159 | "outputs": [], 160 | "source": [ 161 | "function dynamics_rk4(x, u)\n", 162 | " #RK4 integration with zero-order hold on u\n", 163 | " f1 = RD.dynamics(model, x, u)\n", 164 | " f2 = RD.dynamics(model, x + 0.5*h*f1, u)\n", 165 | " f3 = RD.dynamics(model, x + 0.5*h*f2, u)\n", 166 | " f4 = RD.dynamics(model, x + h*f3, u)\n", 167 | " return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 168 | "end\n", 169 | "\n", 170 | "#True model with friction\n", 171 | "function true_dynamics(model::Cartpole, x, u)\n", 172 | " #Perturb masses of cart + pole\n", 173 | " mc = model.mc + 0.02\n", 174 | " mp = model.mp - 0.01\n", 175 | " \n", 176 | " #Perturb length of pendulum\n", 177 | " l = model.l + 0.005\n", 178 | " g = model.g\n", 179 | "\n", 180 | " q = x[ @SVector [1,2] ]\n", 181 | " qd = x[ @SVector [3,4] ]\n", 182 | "\n", 183 | " s = sin(q[2])\n", 184 | " c = cos(q[2])\n", 185 | "\n", 186 | " H = @SMatrix [mc+mp mp*l*c; mp*l*c mp*l^2]\n", 187 | " C = @SMatrix [0 -mp*qd[2]*l*s; 0 0]\n", 188 | " G = @SVector [0, mp*g*l*s]\n", 189 | " B = @SVector [1, 0]\n", 190 | " \n", 191 | " F = [0.01; 0.01].*tanh.(5.0*qd) #nonlinear friction (≈Coulomb/stiction)\n", 192 | "\n", 193 | " qdd = -H\\(C*qd + F + G - B*u[1])\n", 194 | " return [qd; qdd]\n", 195 | "end\n", 196 | "\n", 197 | "function true_dynamics_rk4(x,u)\n", 198 | " #RK4 integration with zero-order hold on u\n", 199 | " f1 = true_dynamics(model, x, u)\n", 200 | " f2 = true_dynamics(model, x + 0.5*h*f1, u)\n", 201 | " f3 = true_dynamics(model, x + 0.5*h*f2, u)\n", 202 | " f4 = true_dynamics(model, x + h*f3, u)\n", 203 | " return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 204 | "end" 205 | ] 206 | }, 207 | { 208 | "cell_type": "code", 209 | "execution_count": null, 210 | "metadata": {}, 211 | "outputs": [], 212 | "source": [ 213 | "#Compute A and B matrices using nominal dynamics + trajectory\n", 214 | "A = zeros(n,n,N-1)\n", 215 | "B = zeros(n,m,N-1)\n", 216 | "for k = 1:(N-1)\n", 217 | " A[:,:,k] .= ForwardDiff.jacobian(x->dynamics_rk4(x,Uopt[k]),Xopt[k])\n", 218 | " B[:,:,k] .= ForwardDiff.jacobian(u->dynamics_rk4(Xopt[k],u),Uopt[k])\n", 219 | "end" 220 | ] 221 | }, 222 | { 223 | "cell_type": "code", 224 | "execution_count": null, 225 | "metadata": {}, 226 | "outputs": [], 227 | "source": [ 228 | "#Compute LQR Tracking Controller\n", 229 | "P = zeros(n,n,N)\n", 230 | "K = zeros(m,n,N-1)\n", 231 | "P[:,:,N] = Qf\n", 232 | "for k = (N-1):-1:1\n", 233 | " K[:,:,k] .= (R + B[:,:,k]'*P[:,:,k+1]*B[:,:,k])\\(B[:,:,k]'*P[:,:,k+1]*A[:,:,k])\n", 234 | " P[:,:,k] .= Q + K[:,:,k]'*R*K[:,:,k] + (A[:,:,k]-B[:,:,k]*K[:,:,k])'*P[:,:,k+1]*(A[:,:,k]-B[:,:,k]*K[:,:,k])\n", 235 | "end" 236 | ] 237 | }, 238 | { 239 | "cell_type": "code", 240 | "execution_count": null, 241 | "metadata": {}, 242 | "outputs": [], 243 | "source": [ 244 | "#Rollout with true dynamics\n", 245 | "xtraj = zeros(n,N)\n", 246 | "for k = 1:(N-1)\n", 247 | " utraj[:,k] .= unom[:,k] - K[:,:,k]*(xtraj[:,k]-xnom[:,k]) #with LQR tracking controller\n", 248 | " utraj[:,k] .= min.(u_bnd, max.(-u_bnd, utraj[:,k])) #clip values within torque limits\n", 249 | " xtraj[:,k+1] .= true_dynamics_rk4(xtraj[:,k], utraj[:,k])\n", 250 | "end" 251 | ] 252 | }, 253 | { 254 | "cell_type": "code", 255 | "execution_count": null, 256 | "metadata": {}, 257 | "outputs": [], 258 | "source": [ 259 | "plot(xtraj[1,:])\n", 260 | "plot(xnom[1,:])" 261 | ] 262 | }, 263 | { 264 | "cell_type": "code", 265 | "execution_count": null, 266 | "metadata": {}, 267 | "outputs": [], 268 | "source": [ 269 | "plot(xtraj[2,:])\n", 270 | "plot(xnom[2,:])" 271 | ] 272 | }, 273 | { 274 | "cell_type": "code", 275 | "execution_count": null, 276 | "metadata": {}, 277 | "outputs": [], 278 | "source": [ 279 | "plot(utraj[1,:])\n", 280 | "plot(Uopt[:])" 281 | ] 282 | }, 283 | { 284 | "cell_type": "code", 285 | "execution_count": null, 286 | "metadata": {}, 287 | "outputs": [], 288 | "source": [ 289 | "#Build matrices for ILC QP\n", 290 | "Nh = N\n", 291 | "#Cost\n", 292 | "Qilc = sparse(Diagonal([0.01; 0; 1.0; 0]))\n", 293 | "Rilc = sparse(Diagonal([.1]))\n", 294 | "H = blockdiag(kron(I(Nh-2), blockdiag(Rilc, Qilc)), Rilc, sparse(Qf))\n", 295 | "q = zeros((n+m)*(Nh-1))\n", 296 | "for k = 1:(Nh-2)\n", 297 | " q[(k-1)*(m+n) .+ (1:(m+n))] .= [0.0; Qilc*(xtraj[:,k+1]-Xopt[k+1])]\n", 298 | "end\n", 299 | "q[(Nh-2)*(m+n) .+ (1:(m+n))] .= [0.0; Qf*(xtraj[:,Nh]-Xopt[Nh])]\n", 300 | "\n", 301 | "#Constraints\n", 302 | "U = kron(I(Nh-1), [I zeros(m,n)]) #Matrix that picks out all u\n", 303 | "X = kron(I(Nh-1), [zeros(n,m) I]) #Matrix that picks out all x\n", 304 | "D = spzeros(n*(Nh-1), (n+m)*(Nh-1)) #dynamics constraints\n", 305 | "\n", 306 | "D[1:n,1:m] .= B[:,:,1]\n", 307 | "D[1:n,(m+1):(m+n)] .= -I(n)\n", 308 | "for k = 1:(Nh-2)\n", 309 | " D[(k*n).+(1:n), (m+(k-1)*(n+m)).+(1:(2*n+m))] .= [A[:,:,k+1] B[:,:,k+1] -I]\n", 310 | "end\n", 311 | "\n", 312 | "lb = [zeros(n*(Nh-1)); -u_bnd.-utraj[1:(Nh-1)]]\n", 313 | "ub = [zeros(n*(Nh-1)); u_bnd.-utraj[1:(Nh-1)]]\n", 314 | "\n", 315 | "qp = OSQP.Model()\n", 316 | "OSQP.setup!(qp, P=H, q=q, A=[D; U], l=lb, u=ub, eps_abs=1e-6, eps_rel=1e-6, eps_prim_inf = 1.0e-6, eps_dual_inf = 1.0e-6, polish=1)\n", 317 | "results = OSQP.solve!(qp)\n", 318 | "ztraj = results.x;" 319 | ] 320 | }, 321 | { 322 | "cell_type": "code", 323 | "execution_count": null, 324 | "metadata": {}, 325 | "outputs": [], 326 | "source": [ 327 | "Δu = U*ztraj\n", 328 | "utraj[1:(Nh-1)] .= utraj[1:(Nh-1)]+Δu" 329 | ] 330 | }, 331 | { 332 | "cell_type": "code", 333 | "execution_count": null, 334 | "metadata": {}, 335 | "outputs": [], 336 | "source": [] 337 | } 338 | ], 339 | "metadata": { 340 | "kernelspec": { 341 | "display_name": "Julia 1.6.7", 342 | "language": "julia", 343 | "name": "julia-1.6" 344 | }, 345 | "language_info": { 346 | "file_extension": ".jl", 347 | "mimetype": "application/julia", 348 | "name": "julia", 349 | "version": "1.6.7" 350 | } 351 | }, 352 | "nbformat": 4, 353 | "nbformat_minor": 4 354 | } 355 | -------------------------------------------------------------------------------- /Lecture 19/Lecture 19.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 19/Lecture 19.pdf -------------------------------------------------------------------------------- /Lecture 19/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e" 3 | -------------------------------------------------------------------------------- /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,:])\n", 205 | "plot(x̂hist[1,:])" 206 | ] 207 | }, 208 | { 209 | "cell_type": "code", 210 | "execution_count": null, 211 | "id": "6fe7606d-3ba8-48a0-84c7-26890e2d393e", 212 | "metadata": {}, 213 | "outputs": [], 214 | "source": [ 215 | "#Covariance and Kalman gain converge to steady-state values in the LTI case\n", 216 | "#(just like LQR gain + cost-to-go Hessian)\n", 217 | "plot(Σhist[1,1,1:N-1])\n", 218 | "plot(Σhist[2,2,1:N-1])\n", 219 | "plot(Σhist[1,2,1:N-1])" 220 | ] 221 | }, 222 | { 223 | "cell_type": "code", 224 | "execution_count": null, 225 | "id": "76692abb-efbe-41f2-a89a-d7e17f1ae7e0", 226 | "metadata": {}, 227 | "outputs": [], 228 | "source": [] 229 | } 230 | ], 231 | "metadata": { 232 | "kernelspec": { 233 | "display_name": "Julia 1.6.7", 234 | "language": "julia", 235 | "name": "julia-1.6" 236 | }, 237 | "language_info": { 238 | "file_extension": ".jl", 239 | "mimetype": "application/julia", 240 | "name": "julia", 241 | "version": "1.6.7" 242 | } 243 | }, 244 | "nbformat": 4, 245 | "nbformat_minor": 5 246 | } 247 | -------------------------------------------------------------------------------- /Lecture 2/Lecture 2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 2/Lecture 2.pdf -------------------------------------------------------------------------------- /Lecture 2/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 2/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 2/Project.toml -------------------------------------------------------------------------------- /Lecture 2/integrators.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" 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 pendulum_forward_euler(fun, x0, Tf, h) \n", 49 | " t = Array(range(0,Tf,step=h))\n", 50 | " \n", 51 | " x_hist = zeros(length(x0),length(t))\n", 52 | " x_hist[:,1] .= x0\n", 53 | " \n", 54 | " for k = 1:(length(t)-1)\n", 55 | " x_hist[:,k+1] .= x_hist[:,k] + h*fun(x_hist[:,k])\n", 56 | " end\n", 57 | " \n", 58 | " return x_hist, t\n", 59 | "end" 60 | ] 61 | }, 62 | { 63 | "cell_type": "code", 64 | "execution_count": null, 65 | "metadata": {}, 66 | "outputs": [], 67 | "source": [ 68 | "x0 = [.1; 0]\n", 69 | "x_hist1, t_hist1 = pendulum_forward_euler(pendulum_dynamics, x0, 5, .1)\n", 70 | "plot(t_hist1, x_hist1[1,:])" 71 | ] 72 | }, 73 | { 74 | "cell_type": "code", 75 | "execution_count": null, 76 | "metadata": {}, 77 | "outputs": [], 78 | "source": [ 79 | "function pendulum_euler_Ad(x0, h)\n", 80 | " g = 9.81\n", 81 | " Ad = [1 h; -g*h*cos(x0[1]) 1]\n", 82 | "end" 83 | ] 84 | }, 85 | { 86 | "cell_type": "code", 87 | "execution_count": null, 88 | "metadata": {}, 89 | "outputs": [], 90 | "source": [ 91 | "eigvals(pendulum_euler_Ad(0, 0.001))" 92 | ] 93 | }, 94 | { 95 | "cell_type": "code", 96 | "execution_count": null, 97 | "metadata": {}, 98 | "outputs": [], 99 | "source": [ 100 | "eignorm = zeros(100)\n", 101 | "h = LinRange(0,0.1,100)\n", 102 | "for k = 1:length(eignorm)\n", 103 | " eignorm[k] = max(norm.(eigvals(pendulum_euler_Ad([0;0], h[k])))...)\n", 104 | "end\n", 105 | "plot(h,eignorm)" 106 | ] 107 | }, 108 | { 109 | "cell_type": "code", 110 | "execution_count": null, 111 | "metadata": {}, 112 | "outputs": [], 113 | "source": [ 114 | "function fd_pendulum_rk4(xk, h)\n", 115 | " f1 = pendulum_dynamics(xk)\n", 116 | " f2 = pendulum_dynamics(xk + 0.5*h*f1)\n", 117 | " f3 = pendulum_dynamics(xk + 0.5*h*f2)\n", 118 | " f4 = pendulum_dynamics(xk + h*f3)\n", 119 | " return xk + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 120 | "end" 121 | ] 122 | }, 123 | { 124 | "cell_type": "code", 125 | "execution_count": null, 126 | "metadata": {}, 127 | "outputs": [], 128 | "source": [ 129 | "function pendulum_rk4(fun, x0, Tf, h) \n", 130 | " t = Array(range(0,Tf,step=h))\n", 131 | " \n", 132 | " x_hist = zeros(length(x0),length(t))\n", 133 | " x_hist[:,1] .= x0\n", 134 | " \n", 135 | " for k = 1:(length(t)-1)\n", 136 | " x_hist[:,k+1] .= fd_pendulum_rk4(x_hist[:,k], h)\n", 137 | " end\n", 138 | " \n", 139 | " return x_hist, t\n", 140 | "end" 141 | ] 142 | }, 143 | { 144 | "cell_type": "code", 145 | "execution_count": null, 146 | "metadata": {}, 147 | "outputs": [], 148 | "source": [ 149 | "x0 = [.1; 0]\n", 150 | "x_hist2, t_hist2 = pendulum_rk4(pendulum_dynamics, x0, 10, 0.1)\n", 151 | "plot(t_hist2, x_hist2[1,:])" 152 | ] 153 | }, 154 | { 155 | "cell_type": "code", 156 | "execution_count": null, 157 | "metadata": {}, 158 | "outputs": [], 159 | "source": [ 160 | "using ForwardDiff\n", 161 | "Ad = ForwardDiff.jacobian(x -> fd_pendulum_rk4(x, 0.1), [0; 0])\n", 162 | "norm.(eigvals(Ad))" 163 | ] 164 | }, 165 | { 166 | "cell_type": "code", 167 | "execution_count": null, 168 | "metadata": {}, 169 | "outputs": [], 170 | "source": [ 171 | "eignorm = zeros(100)\n", 172 | "h = LinRange(0,1,100)\n", 173 | "for k = 1:length(eignorm)\n", 174 | " eignorm[k] = max(norm.(eigvals(ForwardDiff.jacobian(x -> fd_pendulum_rk4(x, h[k]), [0; 0])))...)\n", 175 | "end\n", 176 | "plot(h,eignorm)" 177 | ] 178 | }, 179 | { 180 | "cell_type": "code", 181 | "execution_count": null, 182 | "metadata": {}, 183 | "outputs": [], 184 | "source": [ 185 | "function pendulum_backward_euler(fun, x0, Tf, dt)\n", 186 | " t = Array(range(0,Tf,step=dt))\n", 187 | " \n", 188 | " x_hist = zeros(length(x0),length(t))\n", 189 | " x_hist[:,1] .= x0\n", 190 | " \n", 191 | " for k = 1:(length(t)-1)\n", 192 | " e = 1\n", 193 | " x_hist[:,k+1] = x_hist[:,k]\n", 194 | " while e > 1e-8\n", 195 | " xn = x_hist[:,k] + dt.*fun(x_hist[:,k+1])\n", 196 | " e = norm(xn - x_hist[:,k+1])\n", 197 | " x_hist[:,k+1] .= xn\n", 198 | " end\n", 199 | " end\n", 200 | " \n", 201 | " return x_hist, t\n", 202 | "end" 203 | ] 204 | }, 205 | { 206 | "cell_type": "code", 207 | "execution_count": null, 208 | "metadata": {}, 209 | "outputs": [], 210 | "source": [ 211 | "x0 = [.1; 0]\n", 212 | "x_hist3, t_hist3 = pendulum_backward_euler(pendulum_dynamics, x0, 10, 0.01)\n", 213 | "plot(t_hist3, x_hist3[1,:])" 214 | ] 215 | }, 216 | { 217 | "cell_type": "code", 218 | "execution_count": null, 219 | "metadata": {}, 220 | "outputs": [], 221 | "source": [] 222 | } 223 | ], 224 | "metadata": { 225 | "kernelspec": { 226 | "display_name": "Julia 1.6.7", 227 | "language": "julia", 228 | "name": "julia-1.6" 229 | }, 230 | "language_info": { 231 | "file_extension": ".jl", 232 | "mimetype": "application/julia", 233 | "name": "julia", 234 | "version": "1.6.7" 235 | } 236 | }, 237 | "nbformat": 4, 238 | "nbformat_minor": 4 239 | } 240 | -------------------------------------------------------------------------------- /Lecture 20/Lecture 20.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 20/Lecture 20.pdf -------------------------------------------------------------------------------- /Lecture 21/Lecture 21.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 21/Lecture 21.pdf -------------------------------------------------------------------------------- /Lecture 22/Lecture 22.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 22/Lecture 22.pdf -------------------------------------------------------------------------------- /Lecture 23/Lecture 23.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 23/Lecture 23.pdf -------------------------------------------------------------------------------- /Lecture 24/Lecture 24.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 24/Lecture 24.pdf -------------------------------------------------------------------------------- /Lecture 24/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e" 3 | ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" 4 | PyPlot = "d330b81b-6aea-500a-939a-2ce795aea3ee" 5 | -------------------------------------------------------------------------------- /Lecture 3/Lecture 3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 3/Lecture 3.pdf -------------------------------------------------------------------------------- /Lecture 3/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 3/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 3/Project.toml -------------------------------------------------------------------------------- /Lecture 4/Lecture 4.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 4/Lecture 4.pdf -------------------------------------------------------------------------------- /Lecture 4/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 4/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 4/Project.toml -------------------------------------------------------------------------------- /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 = [-3; 2]\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,:], \"rx\")" 119 | ] 120 | }, 121 | { 122 | "cell_type": "code", 123 | "execution_count": null, 124 | "metadata": {}, 125 | "outputs": [], 126 | "source": [ 127 | "H = ∇2f(xguess[:,end]) + ForwardDiff.jacobian(x -> ∂c(x)'*λguess[end], xguess[:,end])" 128 | ] 129 | }, 130 | { 131 | "cell_type": "code", 132 | "execution_count": null, 133 | "metadata": {}, 134 | "outputs": [], 135 | "source": [] 136 | }, 137 | { 138 | "cell_type": "code", 139 | "execution_count": null, 140 | "metadata": {}, 141 | "outputs": [], 142 | "source": [ 143 | "function gauss_newton_step(x0,λ0)\n", 144 | " H = ∇2f(x0)\n", 145 | " C = ∂c(x0)\n", 146 | " Δz = [H C'; C 0]\\[-∇f(x0)-C'*λ0; -c(x0)]\n", 147 | " Δx = Δz[1:2]\n", 148 | " Δλ = Δz[3]\n", 149 | " return x0+Δx, λ0+Δλ\n", 150 | "end" 151 | ] 152 | }, 153 | { 154 | "cell_type": "code", 155 | "execution_count": null, 156 | "metadata": {}, 157 | "outputs": [], 158 | "source": [ 159 | "xguess = [-3; 2]\n", 160 | "λguess = [0.0]\n", 161 | "plot_landscape()\n", 162 | "plot(xguess[1], xguess[2], \"rx\")" 163 | ] 164 | }, 165 | { 166 | "cell_type": "code", 167 | "execution_count": null, 168 | "metadata": {}, 169 | "outputs": [], 170 | "source": [ 171 | "xnew, λnew = gauss_newton_step(xguess[:,end],λguess[end])\n", 172 | "xguess = [xguess xnew]\n", 173 | "λguess = [λguess λnew]\n", 174 | "plot_landscape()\n", 175 | "plot(xguess[1,:], xguess[2,:], \"rx\")" 176 | ] 177 | }, 178 | { 179 | "cell_type": "code", 180 | "execution_count": null, 181 | "metadata": {}, 182 | "outputs": [], 183 | "source": [] 184 | }, 185 | { 186 | "cell_type": "code", 187 | "execution_count": null, 188 | "metadata": {}, 189 | "outputs": [], 190 | "source": [] 191 | } 192 | ], 193 | "metadata": { 194 | "kernelspec": { 195 | "display_name": "Julia 1.6.7", 196 | "language": "julia", 197 | "name": "julia-1.6" 198 | }, 199 | "language_info": { 200 | "file_extension": ".jl", 201 | "mimetype": "application/julia", 202 | "name": "julia", 203 | "version": "1.6.7" 204 | } 205 | }, 206 | "nbformat": 4, 207 | "nbformat_minor": 4 208 | } 209 | -------------------------------------------------------------------------------- /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)*Δx\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.7", 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.7" 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-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 5/Lecture 5.pdf -------------------------------------------------------------------------------- /Lecture 5/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 5/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 5/Project.toml -------------------------------------------------------------------------------- /Lecture 5/inequality-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 | "A = [1.0 -1.0]\n", 48 | "b = -1.0\n", 49 | "function c(x)\n", 50 | " return dot(A,x) - b\n", 51 | "end\n", 52 | "function ∂c(x)\n", 53 | " return A\n", 54 | "end" 55 | ] 56 | }, 57 | { 58 | "cell_type": "code", 59 | "execution_count": null, 60 | "metadata": {}, 61 | "outputs": [], 62 | "source": [ 63 | "function plot_landscape()\n", 64 | " Nsamp = 20\n", 65 | " Xsamp = kron(ones(Nsamp),LinRange(-4,4,Nsamp)')\n", 66 | " Ysamp = kron(ones(Nsamp)',LinRange(-4,4,Nsamp))\n", 67 | " Zsamp = zeros(Nsamp,Nsamp)\n", 68 | " for j = 1:Nsamp\n", 69 | " for k = 1:Nsamp\n", 70 | " Zsamp[j,k] = f([Xsamp[j,k]; Ysamp[j,k]])\n", 71 | " end\n", 72 | " end\n", 73 | " contour(Xsamp,Ysamp,Zsamp)\n", 74 | "\n", 75 | " xc = LinRange(-4,3,Nsamp)\n", 76 | " plot(xc,xc.+1,\"y\")\n", 77 | "end\n", 78 | "\n", 79 | "plot_landscape()" 80 | ] 81 | }, 82 | { 83 | "cell_type": "code", 84 | "execution_count": null, 85 | "metadata": {}, 86 | "outputs": [], 87 | "source": [ 88 | "function La(x,λ,ρ)\n", 89 | " p = max(0,c(x))\n", 90 | " return f(x) + λ*p + (ρ/2)*(p'*p)\n", 91 | "end" 92 | ] 93 | }, 94 | { 95 | "cell_type": "code", 96 | "execution_count": null, 97 | "metadata": {}, 98 | "outputs": [], 99 | "source": [ 100 | "function newton_solve(x0,λ,ρ)\n", 101 | " x = x0\n", 102 | " p = max.(0,c(x))\n", 103 | " C = zeros(1,2)\n", 104 | " if c(x) ≥ 0\n", 105 | " C = ∂c(x)\n", 106 | " end\n", 107 | " g = ∇f(x) + (λ+ρ*p)*C'\n", 108 | " while norm(g) ≥ 1e-8\n", 109 | " H = ∇2f(x) + ρ*C'*C\n", 110 | " Δx = -H\\g\n", 111 | " \n", 112 | " x = x+Δx\n", 113 | " \n", 114 | " p = max.(0,c(x))\n", 115 | " C = zeros(1,2)\n", 116 | " if c(x) ≥ 0\n", 117 | " C = ∂c(x)\n", 118 | " end\n", 119 | " g = ∇f(x) + (λ+ρ*p)*C'\n", 120 | " end\n", 121 | " \n", 122 | " return x\n", 123 | "end" 124 | ] 125 | }, 126 | { 127 | "cell_type": "code", 128 | "execution_count": null, 129 | "metadata": {}, 130 | "outputs": [], 131 | "source": [ 132 | "xguess = [-3; 2]\n", 133 | "λguess = [0.0]\n", 134 | "ρ = 1.0\n", 135 | "plot_landscape()\n", 136 | "plot(xguess[1], xguess[2], \"rx\")" 137 | ] 138 | }, 139 | { 140 | "cell_type": "code", 141 | "execution_count": null, 142 | "metadata": {}, 143 | "outputs": [], 144 | "source": [ 145 | "xnew = newton_solve(xguess[:,end],λguess[end],ρ)\n", 146 | "λnew = max.(0,λguess[end]+ρ*c(xnew))\n", 147 | "xguess = [xguess xnew]\n", 148 | "λguess = [λguess λnew]\n", 149 | "ρ = 10*ρ\n", 150 | "plot_landscape()\n", 151 | "plot(xguess[1,:], xguess[2,:], \"rx\")" 152 | ] 153 | }, 154 | { 155 | "cell_type": "code", 156 | "execution_count": null, 157 | "metadata": {}, 158 | "outputs": [], 159 | "source": [] 160 | } 161 | ], 162 | "metadata": { 163 | "kernelspec": { 164 | "display_name": "Julia 1.6.7", 165 | "language": "julia", 166 | "name": "julia-1.6" 167 | }, 168 | "language_info": { 169 | "file_extension": ".jl", 170 | "mimetype": "application/julia", 171 | "name": "julia", 172 | "version": "1.6.7" 173 | } 174 | }, 175 | "nbformat": 4, 176 | "nbformat_minor": 4 177 | } 178 | -------------------------------------------------------------------------------- /Lecture 5/merit-functions.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 | "plot_landscape()" 77 | ] 78 | }, 79 | { 80 | "cell_type": "code", 81 | "execution_count": null, 82 | "metadata": {}, 83 | "outputs": [], 84 | "source": [ 85 | "function gauss_newton_step(x,λ)\n", 86 | " H = ∇2f(x)\n", 87 | " C = ∂c(x)\n", 88 | " Δz = [H C'; C 0]\\[-∇f(x)-C'*λ; -c(x)]\n", 89 | " Δx = Δz[1:2]\n", 90 | " Δλ = Δz[3]\n", 91 | " return Δx, Δλ\n", 92 | "end" 93 | ] 94 | }, 95 | { 96 | "cell_type": "code", 97 | "execution_count": null, 98 | "metadata": {}, 99 | "outputs": [], 100 | "source": [ 101 | "xguess = [-1; -1]\n", 102 | "λguess = [0.0]\n", 103 | "plot_landscape()\n", 104 | "plot(xguess[1], xguess[2], \"rx\")" 105 | ] 106 | }, 107 | { 108 | "cell_type": "code", 109 | "execution_count": null, 110 | "metadata": {}, 111 | "outputs": [], 112 | "source": [ 113 | "Δx, Δλ = gauss_newton_step(xguess[:,end],λguess[end])\n", 114 | "xguess = [xguess xguess[:,end]+Δx]\n", 115 | "λguess = [λguess λguess[end]+Δλ]\n", 116 | "plot_landscape()\n", 117 | "plot(xguess[1,:], xguess[2,:], \"rx\")" 118 | ] 119 | }, 120 | { 121 | "cell_type": "code", 122 | "execution_count": null, 123 | "metadata": {}, 124 | "outputs": [], 125 | "source": [ 126 | "function P(x,λ)\n", 127 | " ∇L = [-∇f(x)-∂c(x)'*λ; -c(x)]\n", 128 | " return 0.5*dot(∇L,∇L)\n", 129 | "end\n", 130 | "function ∇P(x,λ)\n", 131 | " H = ∇2f(x) + ForwardDiff.jacobian(xn -> ∂c(xn)'*λ, x)\n", 132 | " C = ∂c(x)\n", 133 | " return [H C'; C 0]*[-∇f(x)-C'*λ; -c(x)]\n", 134 | "end" 135 | ] 136 | }, 137 | { 138 | "cell_type": "code", 139 | "execution_count": null, 140 | "metadata": {}, 141 | "outputs": [], 142 | "source": [ 143 | "ρ = 1.0\n", 144 | "function P(x,λ)\n", 145 | " f(x) + ρ*norm(c(x),1)\n", 146 | "end\n", 147 | "function ∇P(x,λ)\n", 148 | " [∇f(x) + ρ*∂c(x)'*sign.(c(x)); zeros(length(λ))]\n", 149 | "end" 150 | ] 151 | }, 152 | { 153 | "cell_type": "code", 154 | "execution_count": null, 155 | "metadata": {}, 156 | "outputs": [], 157 | "source": [ 158 | "ρ = 1.0\n", 159 | "function P(x,λ)\n", 160 | " f(x) + λ'*c(x) + 0.5*ρ*dot(c(x),c(x))\n", 161 | "end\n", 162 | "function ∇P(x,λ)\n", 163 | " [∇f(x) + ∂c(x)'*(λ + ρ*c(x)); c(x)]\n", 164 | "end" 165 | ] 166 | }, 167 | { 168 | "cell_type": "code", 169 | "execution_count": null, 170 | "metadata": {}, 171 | "outputs": [], 172 | "source": [ 173 | "Δx, Δλ = gauss_newton_step(xguess[:,end],λguess[end])\n", 174 | "α = 1\n", 175 | "while P(xguess[:,end]+α*Δx, λguess[end]+α*Δλ) > P(xguess[:,end], λguess[end]) + 0.01*α*dot(∇P(xguess[:,end], λguess[end]),[Δx; Δλ])\n", 176 | " α = 0.5*α\n", 177 | "end\n", 178 | "xguess = [xguess xguess[:,end]+α*Δx]\n", 179 | "λguess = [λguess λguess[end]+α*Δλ]\n", 180 | "plot_landscape()\n", 181 | "plot(xguess[1,:], xguess[2,:], \"rx\")\n", 182 | "α" 183 | ] 184 | }, 185 | { 186 | "cell_type": "code", 187 | "execution_count": null, 188 | "metadata": {}, 189 | "outputs": [], 190 | "source": [] 191 | } 192 | ], 193 | "metadata": { 194 | "kernelspec": { 195 | "display_name": "Julia 1.6.7", 196 | "language": "julia", 197 | "name": "julia-1.6" 198 | }, 199 | "language_info": { 200 | "file_extension": ".jl", 201 | "mimetype": "application/julia", 202 | "name": "julia", 203 | "version": "1.6.7" 204 | } 205 | }, 206 | "nbformat": 4, 207 | "nbformat_minor": 4 208 | } 209 | -------------------------------------------------------------------------------- /Lecture 5/regularization.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 | "plot_landscape()" 77 | ] 78 | }, 79 | { 80 | "cell_type": "code", 81 | "execution_count": null, 82 | "metadata": {}, 83 | "outputs": [], 84 | "source": [ 85 | "function newton_step(x,λ)\n", 86 | " H = ∇2f(x) + ForwardDiff.jacobian(xn -> ∂c(xn)'*λ, x)\n", 87 | " C = ∂c(x)\n", 88 | " K = [H C'; C 0]\n", 89 | " Δz = K\\[-∇f(x)-C'*λ; -c(x)]\n", 90 | " Δx = Δz[1:2]\n", 91 | " Δλ = Δz[3]\n", 92 | " return Δx, Δλ\n", 93 | "end" 94 | ] 95 | }, 96 | { 97 | "cell_type": "code", 98 | "execution_count": null, 99 | "metadata": {}, 100 | "outputs": [], 101 | "source": [ 102 | "xguess = [-3; 2]\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 | "Δx, Δλ = newton_step(xguess[:,end],λguess[end])\n", 115 | "xguess = [xguess xguess[:,end]+Δx]\n", 116 | "λguess = [λguess λguess[end]+Δλ]\n", 117 | "plot_landscape()\n", 118 | "plot(xguess[1,:], xguess[2,:], \"rx\")" 119 | ] 120 | }, 121 | { 122 | "cell_type": "code", 123 | "execution_count": null, 124 | "metadata": {}, 125 | "outputs": [], 126 | "source": [ 127 | "H = ∇2f(xguess[:,end]) + ForwardDiff.jacobian(xn -> ∂c(xn)'*λguess[end], xguess[:,end])\n", 128 | "C = ∂c(xguess[:,end])\n", 129 | "K = [H C'; C 0]\n", 130 | "eigvals(K)" 131 | ] 132 | }, 133 | { 134 | "cell_type": "code", 135 | "execution_count": null, 136 | "metadata": {}, 137 | "outputs": [], 138 | "source": [ 139 | "function regularized_newton_step(x,λ)\n", 140 | " β = 1.0\n", 141 | " H = ∇2f(x) + ForwardDiff.jacobian(xn -> ∂c(xn)'*λ, x)\n", 142 | " C = ∂c(x)\n", 143 | " K = [H C'; C 0]\n", 144 | " e = eigvals(K)\n", 145 | " while !(sum(e .> 0) == length(x) && sum(e .< 0) == length(λ))\n", 146 | " K = K + Diagonal([β*ones(length(x)); -β*ones(length(λ))])\n", 147 | " e = eigvals(K)\n", 148 | " end\n", 149 | " Δz = K\\[-∇f(x)-C'*λ; -c(x)]\n", 150 | " Δx = Δz[1:2]\n", 151 | " Δλ = Δz[3]\n", 152 | " return Δx, Δλ\n", 153 | "end" 154 | ] 155 | }, 156 | { 157 | "cell_type": "code", 158 | "execution_count": null, 159 | "metadata": {}, 160 | "outputs": [], 161 | "source": [ 162 | "xguess = [-3; 2]\n", 163 | "λguess = [0.0]\n", 164 | "plot_landscape()\n", 165 | "plot(xguess[1], xguess[2], \"rx\")" 166 | ] 167 | }, 168 | { 169 | "cell_type": "code", 170 | "execution_count": null, 171 | "metadata": {}, 172 | "outputs": [], 173 | "source": [ 174 | "Δx, Δλ = regularized_newton_step(xguess[:,end],λguess[end])\n", 175 | "xguess = [xguess xguess[:,end]+Δx]\n", 176 | "λguess = [λguess λguess[end]+Δλ]\n", 177 | "plot_landscape()\n", 178 | "plot(xguess[1,:], xguess[2,:], \"rx\")" 179 | ] 180 | }, 181 | { 182 | "cell_type": "code", 183 | "execution_count": null, 184 | "metadata": {}, 185 | "outputs": [], 186 | "source": [] 187 | } 188 | ], 189 | "metadata": { 190 | "kernelspec": { 191 | "display_name": "Julia 1.6.7", 192 | "language": "julia", 193 | "name": "julia-1.6" 194 | }, 195 | "language_info": { 196 | "file_extension": ".jl", 197 | "mimetype": "application/julia", 198 | "name": "julia", 199 | "version": "1.6.7" 200 | } 201 | }, 202 | "nbformat": 4, 203 | "nbformat_minor": 4 204 | } 205 | -------------------------------------------------------------------------------- /Lecture 6/Control History.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 6/Control History.pdf -------------------------------------------------------------------------------- /Lecture 6/Lecture 6.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 6/Lecture 6.pdf -------------------------------------------------------------------------------- /Lecture 7/Lecture 7.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/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": 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" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "# Discrete dynamics\n", 30 | "h = 0.1 # time step\n", 31 | "A = [1 h; 0 1]\n", 32 | "B = [0.5*h*h; h]" 33 | ] 34 | }, 35 | { 36 | "cell_type": "code", 37 | "execution_count": null, 38 | "metadata": {}, 39 | "outputs": [], 40 | "source": [ 41 | "n = 2 # number of state\n", 42 | "m = 1 # number of controls\n", 43 | "Tfinal = 100.0 # final time\n", 44 | "N = Int(Tfinal/h)+1 # number of time steps\n", 45 | "thist = Array(range(0,h*(N-1), step=h));" 46 | ] 47 | }, 48 | { 49 | "cell_type": "code", 50 | "execution_count": null, 51 | "metadata": {}, 52 | "outputs": [], 53 | "source": [ 54 | "# Initial conditions\n", 55 | "x0 = [1.0; 0]" 56 | ] 57 | }, 58 | { 59 | "cell_type": "code", 60 | "execution_count": null, 61 | "metadata": {}, 62 | "outputs": [], 63 | "source": [ 64 | "# Cost weights\n", 65 | "Q = sparse(1.0*I(2))\n", 66 | "R = sparse(0.1*I(1))\n", 67 | "Qn = sparse(1.0*I(2))" 68 | ] 69 | }, 70 | { 71 | "cell_type": "code", 72 | "execution_count": null, 73 | "metadata": {}, 74 | "outputs": [], 75 | "source": [ 76 | "#Cost function\n", 77 | "function J(xhist,uhist)\n", 78 | " cost = 0.5*xhist[:,end]'*Qn*xhist[:,end]\n", 79 | " for k = 1:(N-1)\n", 80 | " cost = cost + 0.5*xhist[:,k]'*Q*xhist[:,k] + 0.5*(uhist[k]'*R*uhist[k])[1]\n", 81 | " end\n", 82 | " return cost\n", 83 | "end" 84 | ] 85 | }, 86 | { 87 | "cell_type": "code", 88 | "execution_count": null, 89 | "metadata": {}, 90 | "outputs": [], 91 | "source": [ 92 | "# Cost\n", 93 | "H = blockdiag(R, kron(I(N-2), blockdiag(Q,R)), Qn);" 94 | ] 95 | }, 96 | { 97 | "cell_type": "code", 98 | "execution_count": null, 99 | "metadata": {}, 100 | "outputs": [], 101 | "source": [ 102 | "# Constraints\n", 103 | "C = kron(I(N-1), [B -I(2)])\n", 104 | "for k = 1:N-2\n", 105 | " C[(k*n).+(1:n), (k*(n+m)-n).+(1:n)] .= A\n", 106 | "end\n", 107 | "d = [-A*x0; zeros(size(C,1)-n)];" 108 | ] 109 | }, 110 | { 111 | "cell_type": "code", 112 | "execution_count": null, 113 | "metadata": {}, 114 | "outputs": [], 115 | "source": [ 116 | "# Solve the linear system\n", 117 | "y = [H C'; C zeros(size(C,1),size(C,1))]\\[zeros(size(H,1)); d]\n", 118 | "\n", 119 | "# Get state history\n", 120 | "z = y[1:size(H,1)] # states and controls [u0,x1,u1,...,xN]\n", 121 | "Z = reshape(z,n+m,N-1)\n", 122 | "xhist = Z[m+1:end,:]\n", 123 | "xhist = [x0 xhist]\n", 124 | "\n", 125 | "# Get control history\n", 126 | "uhist = Z[1,:];" 127 | ] 128 | }, 129 | { 130 | "cell_type": "code", 131 | "execution_count": null, 132 | "metadata": {}, 133 | "outputs": [], 134 | "source": [ 135 | "# Plot x1 vs. x2, u vs. t, x vs. t, etc.\n", 136 | "times = range(0,h*(N-1), step=h)\n", 137 | "plot(times,xhist[1,:], label=\"position\")\n", 138 | "plot(times,xhist[2,:], label=\"velocity\")\n", 139 | "xlabel(\"time\")\n", 140 | "legend()" 141 | ] 142 | }, 143 | { 144 | "cell_type": "code", 145 | "execution_count": null, 146 | "metadata": {}, 147 | "outputs": [], 148 | "source": [ 149 | "plot(times[1:end-1], uhist, label=\"control\")\n", 150 | "xlabel(\"Time\")\n", 151 | "legend()" 152 | ] 153 | }, 154 | { 155 | "cell_type": "code", 156 | "execution_count": null, 157 | "metadata": {}, 158 | "outputs": [], 159 | "source": [ 160 | "J(xhist,uhist)" 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.7", 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.7" 182 | } 183 | }, 184 | "nbformat": 4, 185 | "nbformat_minor": 4 186 | } 187 | -------------------------------------------------------------------------------- /Lecture 7/lqr-riccati.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 ControlSystems" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "# Discrete dynamics\n", 30 | "h = 0.1 # time step\n", 31 | "A = [1 h; 0 1]\n", 32 | "B = [0.5*h*h; h]" 33 | ] 34 | }, 35 | { 36 | "cell_type": "code", 37 | "execution_count": null, 38 | "metadata": {}, 39 | "outputs": [], 40 | "source": [ 41 | "n = 2 # number of state\n", 42 | "m = 1 # number of controls\n", 43 | "Tfinal = 10.0 # final time\n", 44 | "N = Int(Tfinal/h)+1 # number of time steps\n", 45 | "thist = Array(range(0,h*(N-1), step=h));" 46 | ] 47 | }, 48 | { 49 | "cell_type": "code", 50 | "execution_count": null, 51 | "metadata": {}, 52 | "outputs": [], 53 | "source": [ 54 | "# Initial conditions\n", 55 | "x0 = [1.0; 0]" 56 | ] 57 | }, 58 | { 59 | "cell_type": "code", 60 | "execution_count": null, 61 | "metadata": {}, 62 | "outputs": [], 63 | "source": [ 64 | "# Cost weights\n", 65 | "Q = Array(1.0*I(2))\n", 66 | "R = 0.1 #Array(1.0*I(1))\n", 67 | "Qn = Array(1.0*I(2))" 68 | ] 69 | }, 70 | { 71 | "cell_type": "code", 72 | "execution_count": null, 73 | "metadata": {}, 74 | "outputs": [], 75 | "source": [ 76 | "#Cost function\n", 77 | "function J(xhist,uhist)\n", 78 | " cost = 0.5*xhist[:,end]'*Qn*xhist[:,end]\n", 79 | " for k = 1:(N-1)\n", 80 | " cost = cost + 0.5*xhist[:,k]'*Q*xhist[:,k] + 0.5*(uhist[k]'*R*uhist[k])[1]\n", 81 | " end\n", 82 | " return cost\n", 83 | "end" 84 | ] 85 | }, 86 | { 87 | "cell_type": "code", 88 | "execution_count": null, 89 | "metadata": {}, 90 | "outputs": [], 91 | "source": [ 92 | "P = zeros(n,n,N)\n", 93 | "K = zeros(m,n,N-1)\n", 94 | "\n", 95 | "P[:,:,N] .= Qn\n", 96 | "\n", 97 | "#Backward Riccati recursion\n", 98 | "for k = (N-1):-1:1\n", 99 | " K[:,:,k] .= (R + B'*P[:,:,k+1]*B)\\(B'*P[:,:,k+1]*A)\n", 100 | " P[:,:,k] .= Q + A'*P[:,:,k+1]*(A-B*K[:,:,k])\n", 101 | "end\n", 102 | "\n", 103 | "#Forward rollout starting at x0\n", 104 | "xhist = zeros(n,N)\n", 105 | "xhist[:,1] = x0\n", 106 | "uhist = zeros(m,N-1)\n", 107 | "for k = 1:(N-1)\n", 108 | " uhist[:,k] .= -K[:,:,k]*xhist[:,k]\n", 109 | " xhist[:,k+1] .= A*xhist[:,k] + B*uhist[k]\n", 110 | "end" 111 | ] 112 | }, 113 | { 114 | "cell_type": "code", 115 | "execution_count": null, 116 | "metadata": {}, 117 | "outputs": [], 118 | "source": [ 119 | "# Plot x1 vs. x2, u vs. t, x vs. t, etc.\n", 120 | "times = range(0,h*(N-1), step=h)\n", 121 | "plot(times,xhist[1,:], label=\"position\")\n", 122 | "plot(times,xhist[2,:], label=\"velocity\")\n", 123 | "xlabel(\"time\")\n", 124 | "legend()" 125 | ] 126 | }, 127 | { 128 | "cell_type": "code", 129 | "execution_count": null, 130 | "metadata": {}, 131 | "outputs": [], 132 | "source": [ 133 | "plot(times[1:end-1], uhist[1,:], label=\"control\")\n", 134 | "xlabel(\"Time\")\n", 135 | "legend()" 136 | ] 137 | }, 138 | { 139 | "cell_type": "code", 140 | "execution_count": null, 141 | "metadata": {}, 142 | "outputs": [], 143 | "source": [ 144 | "J(xhist,uhist)" 145 | ] 146 | }, 147 | { 148 | "cell_type": "code", 149 | "execution_count": null, 150 | "metadata": {}, 151 | "outputs": [], 152 | "source": [ 153 | "#Forward rollout with random noise\n", 154 | "xhist = zeros(n,N)\n", 155 | "xhist[:,1] = x0 #10.0*randn(2) #Can start anywhere\n", 156 | "uhist = zeros(m,N-1)\n", 157 | "for k = 1:(N-1)\n", 158 | " uhist[:,k] .= -K[:,:,k]*xhist[:,k]\n", 159 | " xhist[:,k+1] .= A*xhist[:,k] + B*uhist[k] + 0.01*randn(2) #can add noise to dynamics\n", 160 | "end" 161 | ] 162 | }, 163 | { 164 | "cell_type": "code", 165 | "execution_count": null, 166 | "metadata": {}, 167 | "outputs": [], 168 | "source": [ 169 | "plot(K[1,1,:], label=\"K1\")\n", 170 | "plot(K[1,2,:], label=\"K2\")\n", 171 | "xlabel(\"time\")\n", 172 | "legend()" 173 | ] 174 | }, 175 | { 176 | "cell_type": "code", 177 | "execution_count": null, 178 | "metadata": {}, 179 | "outputs": [], 180 | "source": [ 181 | "#Compute infinite-horizon K matrix using ControlSystems.jl\n", 182 | "Kinf = dlqr(A,B,Q,R)\n", 183 | "#Compare to ours\n", 184 | "K[:,:,1]-Kinf" 185 | ] 186 | }, 187 | { 188 | "cell_type": "code", 189 | "execution_count": null, 190 | "metadata": {}, 191 | "outputs": [], 192 | "source": [ 193 | "#Forward rollout with constant K\n", 194 | "xhist = zeros(n,N)\n", 195 | "xhist[:,1] = x0 #Can start anywhere\n", 196 | "uhist = zeros(m,N-1)\n", 197 | "for k = 1:(N-1)\n", 198 | " uhist[:,k] .= -Kinf*xhist[:,k]\n", 199 | " xhist[:,k+1] .= A*xhist[:,k] + B*uhist[k] #+ 0.01*randn(2)\n", 200 | "end" 201 | ] 202 | }, 203 | { 204 | "cell_type": "code", 205 | "execution_count": null, 206 | "metadata": {}, 207 | "outputs": [], 208 | "source": [ 209 | "#Closed-loop Eigenvalues\n", 210 | "eigvals(A-B*Kinf)" 211 | ] 212 | }, 213 | { 214 | "cell_type": "code", 215 | "execution_count": null, 216 | "metadata": {}, 217 | "outputs": [], 218 | "source": [] 219 | } 220 | ], 221 | "metadata": { 222 | "kernelspec": { 223 | "display_name": "Julia 1.6.7", 224 | "language": "julia", 225 | "name": "julia-1.6" 226 | }, 227 | "language_info": { 228 | "file_extension": ".jl", 229 | "mimetype": "application/julia", 230 | "name": "julia", 231 | "version": "1.6.7" 232 | } 233 | }, 234 | "nbformat": 4, 235 | "nbformat_minor": 4 236 | } 237 | -------------------------------------------------------------------------------- /Lecture 7/lqr-shooting.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 | "# Discrete dynamics\n", 29 | "h = 0.1 # time step\n", 30 | "A = [1 h; 0 1]\n", 31 | "B = [0.5*h*h; h]" 32 | ] 33 | }, 34 | { 35 | "cell_type": "code", 36 | "execution_count": null, 37 | "metadata": {}, 38 | "outputs": [], 39 | "source": [ 40 | "n = 2 # number of state\n", 41 | "m = 1 # number of controls\n", 42 | "Tfinal = 5.0 # final time #try larger values\n", 43 | "N = Int(Tfinal/h)+1 # number of time steps\n", 44 | "thist = Array(range(0,h*(N-1), step=h));" 45 | ] 46 | }, 47 | { 48 | "cell_type": "code", 49 | "execution_count": null, 50 | "metadata": {}, 51 | "outputs": [], 52 | "source": [ 53 | "# Initial conditions\n", 54 | "x0 = [1.0; 0]" 55 | ] 56 | }, 57 | { 58 | "cell_type": "code", 59 | "execution_count": null, 60 | "metadata": {}, 61 | "outputs": [], 62 | "source": [ 63 | "# Cost weights\n", 64 | "Q = 1.0*I(2)\n", 65 | "R = 0.1\n", 66 | "Qn = 1.0*I(2)" 67 | ] 68 | }, 69 | { 70 | "cell_type": "code", 71 | "execution_count": null, 72 | "metadata": {}, 73 | "outputs": [], 74 | "source": [ 75 | "function J(xhist,uhist)\n", 76 | " cost = 0.5*xhist[:,end]'*Qn*xhist[:,end]\n", 77 | " for k = 1:(N-1)\n", 78 | " cost = cost + 0.5*xhist[:,k]'*Q*xhist[:,k] + 0.5*uhist[k]'*R*uhist[k]\n", 79 | " end\n", 80 | " return cost\n", 81 | "end" 82 | ] 83 | }, 84 | { 85 | "cell_type": "code", 86 | "execution_count": null, 87 | "metadata": {}, 88 | "outputs": [], 89 | "source": [ 90 | "function rollout(xhist, uhist)\n", 91 | " xnew = zeros(size(xhist))\n", 92 | " xnew[:,1] = xhist[:,1]\n", 93 | " for k = 1:(N-1)\n", 94 | " xnew[:,k+1] .= A*xnew[:,k] + B*uhist[k]\n", 95 | " end\n", 96 | " return xnew\n", 97 | "end" 98 | ] 99 | }, 100 | { 101 | "cell_type": "code", 102 | "execution_count": null, 103 | "metadata": {}, 104 | "outputs": [], 105 | "source": [ 106 | "# Initial guess\n", 107 | "xhist = repeat(x0, 1, N)\n", 108 | "uhist = zeros(N-1)\n", 109 | "Δu = ones(N-1)\n", 110 | "λhist = zeros(n,N)\n", 111 | "\n", 112 | "xhist = rollout(xhist, uhist) #initial rollout to get state trajectory\n", 113 | "\n", 114 | "J(xhist,uhist) #Initial cost" 115 | ] 116 | }, 117 | { 118 | "cell_type": "code", 119 | "execution_count": null, 120 | "metadata": {}, 121 | "outputs": [], 122 | "source": [ 123 | "b = 1e-2 #line search tolerance\n", 124 | "α = 1.0\n", 125 | "iter = 0\n", 126 | "while maximum(abs.(Δu[:])) > 1e-2 #terminate when the gradient is small\n", 127 | " \n", 128 | " #Backward pass to compute λ and Δu\n", 129 | " λhist[:,N] .= Qn*xhist[:,N]\n", 130 | " for k = N-1:-1:1\n", 131 | " Δu[k] = -(uhist[k]+R\\B'*λhist[:,k+1])\n", 132 | " λhist[:,k] .= Q*xhist[:,k] + A'*λhist[:,k+1]\n", 133 | " end\n", 134 | " \n", 135 | " #Forward pass with line search to compute x\n", 136 | " α = 1.0\n", 137 | " unew = uhist + α.*Δu\n", 138 | " xnew = rollout(xhist, unew)\n", 139 | " while J(xnew, unew) > J(xhist, uhist) - b*α*Δu[:]'*Δu[:]\n", 140 | " α = 0.5*α\n", 141 | " unew = uhist + α.*Δu\n", 142 | " xnew = rollout(xhist, unew)\n", 143 | " end\n", 144 | " uhist .= unew;\n", 145 | " xhist .= xnew;\n", 146 | " iter += 1\n", 147 | "end" 148 | ] 149 | }, 150 | { 151 | "cell_type": "code", 152 | "execution_count": null, 153 | "metadata": {}, 154 | "outputs": [], 155 | "source": [ 156 | "iter" 157 | ] 158 | }, 159 | { 160 | "cell_type": "code", 161 | "execution_count": null, 162 | "metadata": {}, 163 | "outputs": [], 164 | "source": [ 165 | "J(xhist,uhist) #cost of solution" 166 | ] 167 | }, 168 | { 169 | "cell_type": "code", 170 | "execution_count": null, 171 | "metadata": {}, 172 | "outputs": [], 173 | "source": [ 174 | "# Plot x1 vs. x2, u vs. t, x vs. t, etc.\n", 175 | "plot(thist,xhist[1,:], label=\"Position\")\n", 176 | "plot(thist,xhist[2,:], label=\"Velocity\")\n", 177 | "xlabel(\"Time\")\n", 178 | "legend()" 179 | ] 180 | }, 181 | { 182 | "cell_type": "code", 183 | "execution_count": null, 184 | "metadata": {}, 185 | "outputs": [], 186 | "source": [ 187 | "plot(thist[1:end-1], uhist, label=\"control\")\n", 188 | "xlabel(\"Time\")\n", 189 | "legend()" 190 | ] 191 | }, 192 | { 193 | "cell_type": "code", 194 | "execution_count": null, 195 | "metadata": {}, 196 | "outputs": [], 197 | "source": [] 198 | } 199 | ], 200 | "metadata": { 201 | "kernelspec": { 202 | "display_name": "Julia 1.6.7", 203 | "language": "julia", 204 | "name": "julia-1.6" 205 | }, 206 | "language_info": { 207 | "file_extension": ".jl", 208 | "mimetype": "application/julia", 209 | "name": "julia", 210 | "version": "1.6.7" 211 | } 212 | }, 213 | "nbformat": 4, 214 | "nbformat_minor": 4 215 | } 216 | -------------------------------------------------------------------------------- /Lecture 8/Lecture 8.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 8/Lecture 8.pdf -------------------------------------------------------------------------------- /Lecture 8/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e" 3 | -------------------------------------------------------------------------------- /Lecture 9/Lecture 9.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2023/573bce67fe0acc32633bad3d42276efc70628145/Lecture 9/Lecture 9.pdf -------------------------------------------------------------------------------- /Lecture 9/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" 3 | ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e" 4 | CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298" 5 | MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11" 6 | OSQP = "ab2f91bb-94b4-55e3-9ba0-7f65df51de79" 7 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 8 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 9 | Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" 10 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 11 | -------------------------------------------------------------------------------- /Lecture 9/Untitled.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "1e0b0662-d132-4da7-8167-676861f936c4", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [] 10 | } 11 | ], 12 | "metadata": { 13 | "kernelspec": { 14 | "display_name": "Julia 1.6.7", 15 | "language": "julia", 16 | "name": "julia-1.6" 17 | }, 18 | "language_info": { 19 | "file_extension": ".jl", 20 | "mimetype": "application/julia", 21 | "name": "julia", 22 | "version": "1.6.7" 23 | } 24 | }, 25 | "nbformat": 4, 26 | "nbformat_minor": 5 27 | } 28 | -------------------------------------------------------------------------------- /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 = [10.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 | " for (k,x) in enumerate(X)\n", 341 | " atframe(anim, k) do\n", 342 | " x = X[k]\n", 343 | " visualize!(vis, model, SVector{6}(x)) \n", 344 | " end\n", 345 | " end\n", 346 | " setanimation!(vis, anim)\n", 347 | "end" 348 | ] 349 | }, 350 | { 351 | "cell_type": "code", 352 | "execution_count": null, 353 | "metadata": {}, 354 | "outputs": [], 355 | "source": [ 356 | "vis = Visualizer()\n", 357 | "model = PlanarQuadrotor()\n", 358 | "set_mesh!(vis, model)\n", 359 | "render(vis)" 360 | ] 361 | }, 362 | { 363 | "cell_type": "code", 364 | "execution_count": null, 365 | "metadata": {}, 366 | "outputs": [], 367 | "source": [ 368 | "X1 = [SVector{6}(x) for x in eachcol(xhist1)];\n", 369 | "X2 = [SVector{6}(x) for x in eachcol(xhist2)];" 370 | ] 371 | }, 372 | { 373 | "cell_type": "code", 374 | "execution_count": null, 375 | "metadata": {}, 376 | "outputs": [], 377 | "source": [ 378 | "visualize!(vis, model, thist[end], X1)" 379 | ] 380 | }, 381 | { 382 | "cell_type": "code", 383 | "execution_count": null, 384 | "metadata": {}, 385 | "outputs": [], 386 | "source": [ 387 | "visualize!(vis, model, thist[end], X2)" 388 | ] 389 | }, 390 | { 391 | "cell_type": "code", 392 | "execution_count": null, 393 | "metadata": {}, 394 | "outputs": [], 395 | "source": [] 396 | } 397 | ], 398 | "metadata": { 399 | "kernelspec": { 400 | "display_name": "Julia 1.6.7", 401 | "language": "julia", 402 | "name": "julia-1.6" 403 | }, 404 | "language_info": { 405 | "file_extension": ".jl", 406 | "mimetype": "application/julia", 407 | "name": "julia", 408 | "version": "1.6.7" 409 | } 410 | }, 411 | "nbformat": 4, 412 | "nbformat_minor": 4 413 | } 414 | -------------------------------------------------------------------------------- /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 2023. 4 | 5 |

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

6 | --------------------------------------------------------------------------------