├── .gitignore ├── 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 ├── Manifest.toml ├── Project.toml └── rbsim.ipynb ├── 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 ├── Manifest.toml ├── Project.toml └── cartpole-ilc.ipynb ├── Lecture 18 └── Lecture 18.pdf ├── 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 ├── Manifest.toml ├── Project.toml ├── falling-particle-friction.ipynb └── sho-lqr.ipynb ├── Lecture 3 ├── Lecture 3.pdf ├── Manifest.toml ├── Project.toml ├── minimization.ipynb └── root-finding.ipynb ├── Lecture 4 ├── Lecture 4.pdf ├── Manifest.toml ├── Project.toml ├── equality-constraints.ipynb └── minimization.ipynb ├── Lecture 5 ├── Lecture 5.pdf ├── Manifest.toml ├── Project.toml ├── inequality-constraints.ipynb └── regularization.ipynb ├── Lecture 6 ├── Lecture 6.pdf ├── Manifest.toml ├── Project.toml ├── control-history.pdf └── merit-functions.ipynb ├── Lecture 7 ├── Lecture 7.pdf ├── Manifest.toml ├── Project.toml ├── lqr-qp.ipynb ├── lqr-riccati.ipynb └── lqr-shooting.ipynb ├── Lecture 8 ├── Lecture 8.pdf ├── Manifest.toml ├── Project.toml └── lqr-dp.ipynb ├── Lecture 9 ├── Lecture 9.pdf ├── Manifest.toml ├── Project.toml ├── mpc.ipynb └── quadrotor_scaled.obj └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | *.DS_Store 3 | 4 | *.ipynb_checkpoints/ 5 | -------------------------------------------------------------------------------- /Lecture 1/Intro.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 1/Intro.pdf -------------------------------------------------------------------------------- /Lecture 1/Lecture 1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 1/Lecture 1.pdf -------------------------------------------------------------------------------- /Lecture 10/Lecture 10.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 10/Lecture 10.pdf -------------------------------------------------------------------------------- /Lecture 10/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" 3 | MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11" 4 | -------------------------------------------------------------------------------- /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 MeshCat" 22 | ] 23 | }, 24 | { 25 | "cell_type": "code", 26 | "execution_count": null, 27 | "metadata": {}, 28 | "outputs": [], 29 | "source": [ 30 | "function acrobot_dynamics(x, u)\n", 31 | " g = 9.81\n", 32 | " m1,m2 = [1.,1.]\n", 33 | " l1,l2 = [1.,1.]\n", 34 | " J1,J2 = [1.,1.]\n", 35 | " θ1, θ2 = x[1], x[2]\n", 36 | " θ1dot, θ2dot = x[3], x[4]\n", 37 | " s1,c1 = sincos(θ1)\n", 38 | " s2,c2 = sincos(θ2)\n", 39 | " c12 = cos(θ1 + θ2)\n", 40 | "\n", 41 | " # mass matrix\n", 42 | " m11 = m1*l1^2 + J1 + m2*(l1^2 + l2^2 + 2*l1*l2*c2) + J2\n", 43 | " m12 = m2*(l2^2 + l1*l2*c2 + J2)\n", 44 | " m22 = l2^2*m2 + J2\n", 45 | " M = [m11 m12; m12 m22]\n", 46 | "\n", 47 | " # bias term\n", 48 | " tmp = l1*l2*m2*s2\n", 49 | " b1 = -(2 * θ1dot * θ2dot + θ2dot^2)*tmp\n", 50 | " b2 = tmp * θ1dot^2\n", 51 | " B = [b1, b2]\n", 52 | "\n", 53 | " # friction\n", 54 | " c = 1.0\n", 55 | " C = [c*θ1dot, c*θ2dot]\n", 56 | "\n", 57 | " # gravity term\n", 58 | " g1 = ((m1 + m2)*l2*c1 + m2*l2*c12) * g\n", 59 | " g2 = m2*l2*c12*g\n", 60 | " G = [g1, g2]\n", 61 | "\n", 62 | " # equations of motion\n", 63 | " τ = [0, u[1]]\n", 64 | " θddot = M\\(τ - B - G - C)\n", 65 | " return [θ1dot, θ2dot, θddot[1], θddot[2]]\n", 66 | "end" 67 | ] 68 | }, 69 | { 70 | "cell_type": "code", 71 | "execution_count": null, 72 | "metadata": {}, 73 | "outputs": [], 74 | "source": [ 75 | "function dynamics_rk4(x,u)\n", 76 | " #RK4 integration with zero-order hold on u\n", 77 | " f1 = acrobot_dynamics(x, u)\n", 78 | " f2 = acrobot_dynamics(x + 0.5*h*f1, u)\n", 79 | " f3 = acrobot_dynamics(x + 0.5*h*f2, u)\n", 80 | " f4 = acrobot_dynamics(x + h*f3, u)\n", 81 | " return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 82 | "end" 83 | ] 84 | }, 85 | { 86 | "cell_type": "code", 87 | "execution_count": null, 88 | "metadata": {}, 89 | "outputs": [], 90 | "source": [ 91 | "function dfdx(x,u)\n", 92 | " ForwardDiff.jacobian(dx->dynamics_rk4(dx,u),x)\n", 93 | "end\n", 94 | "\n", 95 | "function dfdu(x,u)\n", 96 | " ForwardDiff.derivative(du->dynamics_rk4(x,du),u)\n", 97 | "end" 98 | ] 99 | }, 100 | { 101 | "cell_type": "code", 102 | "execution_count": null, 103 | "metadata": {}, 104 | "outputs": [], 105 | "source": [ 106 | "h = 0.05 # time step \n", 107 | "Nx = 4 # number of state\n", 108 | "Nu = 1 # number of controls\n", 109 | "Tfinal = 10.0 # final time\n", 110 | "Nt = Int(Tfinal/h)+1 # number of time steps\n", 111 | "thist = Array(range(0,h*(Nt-1), step=h));" 112 | ] 113 | }, 114 | { 115 | "cell_type": "code", 116 | "execution_count": null, 117 | "metadata": {}, 118 | "outputs": [], 119 | "source": [ 120 | "# Cost weights\n", 121 | "Q = Diagonal([1.0*ones(2); 0.1*ones(2)]);\n", 122 | "R = 0.01;\n", 123 | "Qn = Array(100.0*I(Nx));" 124 | ] 125 | }, 126 | { 127 | "cell_type": "code", 128 | "execution_count": null, 129 | "metadata": {}, 130 | "outputs": [], 131 | "source": [ 132 | "function stage_cost(x,u)\n", 133 | " return 0.5*((x-xgoal)'*Q*(x-xgoal)) + 0.5*R*u*u\n", 134 | "end" 135 | ] 136 | }, 137 | { 138 | "cell_type": "code", 139 | "execution_count": null, 140 | "metadata": {}, 141 | "outputs": [], 142 | "source": [ 143 | "function terminal_cost(x)\n", 144 | " return 0.5*(x-xgoal)'*Qn*(x-xgoal)\n", 145 | "end" 146 | ] 147 | }, 148 | { 149 | "cell_type": "code", 150 | "execution_count": null, 151 | "metadata": {}, 152 | "outputs": [], 153 | "source": [ 154 | "function cost(xtraj,utraj)\n", 155 | " J = 0.0\n", 156 | " for k = 1:(Nt-1)\n", 157 | " J += stage_cost(xtraj[:,k],utraj[k])\n", 158 | " end\n", 159 | " J += terminal_cost(xtraj[:,Nt])\n", 160 | " return J\n", 161 | "end" 162 | ] 163 | }, 164 | { 165 | "cell_type": "code", 166 | "execution_count": null, 167 | "metadata": {}, 168 | "outputs": [], 169 | "source": [ 170 | "#Initial guess\n", 171 | "x0 = [-pi/2; 0; 0; 0]\n", 172 | "xgoal = [pi/2; 0; 0; 0]\n", 173 | "xtraj = kron(ones(1,Nt), x0)\n", 174 | "utraj = randn(Nt-1);" 175 | ] 176 | }, 177 | { 178 | "cell_type": "code", 179 | "execution_count": null, 180 | "metadata": {}, 181 | "outputs": [], 182 | "source": [ 183 | "#Initial Rollout\n", 184 | "for k = 1:(Nt-1)\n", 185 | " xtraj[:,k+1] .= dynamics_rk4(xtraj[:,k],utraj[k])\n", 186 | "end\n", 187 | "J = cost(xtraj,utraj)" 188 | ] 189 | }, 190 | { 191 | "cell_type": "code", 192 | "execution_count": null, 193 | "metadata": {}, 194 | "outputs": [], 195 | "source": [ 196 | "#DDP Algorithm\n", 197 | "using Printf\n", 198 | "p = zeros(Nx,Nt)\n", 199 | "P = zeros(Nx,Nx,Nt)\n", 200 | "d = ones(Nt-1)\n", 201 | "K = zeros(Nu,Nx,Nt-1)\n", 202 | "ΔJ = 0.0\n", 203 | "\n", 204 | "xn = zeros(Nx,Nt)\n", 205 | "un = zeros(Nt-1)\n", 206 | "\n", 207 | "gx = zeros(Nx)\n", 208 | "gu = 0.0\n", 209 | "Gxx = zeros(Nx,Nx)\n", 210 | "Guu = 0.0\n", 211 | "Gxu = zeros(Nx)\n", 212 | "Gux = zeros(Nx)\n", 213 | "\n", 214 | "iter = 0\n", 215 | "while maximum(abs.(d[:])) > 1e-3\n", 216 | " iter += 1\n", 217 | " \n", 218 | " p = zeros(Nx,Nt)\n", 219 | " P = zeros(Nx,Nx,Nt)\n", 220 | " d = ones(Nt-1)\n", 221 | " K = zeros(Nu,Nx,Nt-1)\n", 222 | " ΔJ = 0.0\n", 223 | "\n", 224 | " p[:,Nt] = Qn*(xtraj[:,Nt]-xgoal)\n", 225 | " P[:,:,Nt] = Qn\n", 226 | " \n", 227 | " #Backward Pass\n", 228 | " for k = (Nt-1):-1:1\n", 229 | " #Calculate derivatives\n", 230 | " q = Q*(xtraj[:,k]-xgoal)\n", 231 | " r = R*utraj[k]\n", 232 | " \n", 233 | " A = dfdx(xtraj[:,k], utraj[k]) #ForwardDiff.jacobian(dx->dynamics_rk4(dx,utraj[k]),xtraj[:,k])\n", 234 | " B = dfdu(xtraj[:,k], utraj[k]) #ForwardDiff.derivative(du->dynamics_rk4(xtraj[:,k],du),utraj[k])\n", 235 | " \n", 236 | " gx = q + A'*p[:,k+1]\n", 237 | " gu = r + B'*p[:,k+1]\n", 238 | " \n", 239 | " Gxx = Q + A'*P[:,:,k+1]*A \n", 240 | " Guu = R + B'*P[:,:,k+1]*B \n", 241 | " Gxu = A'*P[:,:,k+1]*B \n", 242 | " Gux = B'*P[:,:,k+1]*A\n", 243 | " \n", 244 | " d[k] = Guu\\gu\n", 245 | " K[:,:,k] = Guu\\Gux\n", 246 | " \n", 247 | "\n", 248 | " p[:,k] = gx - K[:,:,k]'*gu + K[:,:,k]'*Guu*d[k] - Gxu*d[k]\n", 249 | " P[:,:,k] = Gxx + K[:,:,k]'*Guu*K[:,:,k] - Gxu*K[:,:,k] - K[:,:,k]'*Gux\n", 250 | " \n", 251 | " ΔJ += gu'*d[k]\n", 252 | " end\n", 253 | "\n", 254 | " #Forward rollout with line search\n", 255 | " xn[:,1] = xtraj[:,1]\n", 256 | " α = 1.0\n", 257 | " for k = 1:(Nt-1)\n", 258 | " un[k] = utraj[k] - α*d[k] - dot(K[:,:,k],xn[:,k]-xtraj[:,k])\n", 259 | " xn[:,k+1] .= dynamics_rk4(xn[:,k],un[k])\n", 260 | " end\n", 261 | " Jn = cost(xn,un)\n", 262 | " \n", 263 | " while isnan(Jn) || Jn > (J - 1e-2*α*ΔJ)\n", 264 | " α = 0.5*α\n", 265 | " for k = 1:(Nt-1)\n", 266 | " un[k] = utraj[k] - α*d[k] - dot(K[:,:,k],xn[:,k]-xtraj[:,k])\n", 267 | " xn[:,k+1] = dynamics_rk4(xn[:,k],un[k])\n", 268 | " end\n", 269 | " Jn = cost(xn,un)\n", 270 | " end\n", 271 | " \n", 272 | " # logging\n", 273 | " if rem(iter - 1, 100) == 0\n", 274 | " @printf \"iter J ΔJ |d| α \\n\"\n", 275 | " @printf \"---------------------------------------------------\\n\"\n", 276 | " end\n", 277 | " if rem(iter - 1, 10) == 0 \n", 278 | " @printf(\"%3d %10.3e %9.2e %9.2e %6.4f \\n\",\n", 279 | " iter, J, ΔJ, maximum(abs.(d[:])), α)\n", 280 | " end\n", 281 | " \n", 282 | " J = Jn\n", 283 | " xtraj .= xn\n", 284 | " utraj .= un\n", 285 | "end" 286 | ] 287 | }, 288 | { 289 | "cell_type": "code", 290 | "execution_count": null, 291 | "metadata": {}, 292 | "outputs": [], 293 | "source": [ 294 | "plot(thist,xtraj[1,:])\n", 295 | "plot(thist,xtraj[2,:])" 296 | ] 297 | }, 298 | { 299 | "cell_type": "code", 300 | "execution_count": null, 301 | "metadata": {}, 302 | "outputs": [], 303 | "source": [ 304 | "plot(thist[1:Nt-1],utraj)" 305 | ] 306 | }, 307 | { 308 | "cell_type": "code", 309 | "execution_count": null, 310 | "metadata": {}, 311 | "outputs": [], 312 | "source": [ 313 | "# Acrobot (doublependulum)\n", 314 | "using Colors\n", 315 | "function build_acrobot!(vis, color=colorant\"blue\", thick=0.05)\n", 316 | " l1,l2 = [1.,1.]\n", 317 | " hinge = MeshCat.Cylinder(MeshCat.Point3f(-0.05,0,0), MeshCat.Point3f(0.05,0,0), 0.05f0)\n", 318 | " dim1 = MeshCat.Vec(thick, thick, l1)\n", 319 | " link1 = MeshCat.HyperRectangle(MeshCat.Vec(-thick/2,-thick/2,0),dim1)\n", 320 | " dim2 = MeshCat.Vec(thick, thick, l2)\n", 321 | " link2 = MeshCat.HyperRectangle(MeshCat.Vec(-thick/2,-thick/2,0),dim2)\n", 322 | " mat1 = MeshPhongMaterial(color=colorant\"grey\")\n", 323 | " mat2 = MeshPhongMaterial(color=color)\n", 324 | " setobject!(vis[\"base\"], hinge, mat1) \n", 325 | " setobject!(vis[\"link1\"], link1, mat2) \n", 326 | " setobject!(vis[\"link1\",\"joint\"], hinge, mat1) \n", 327 | " setobject!(vis[\"link1\",\"link2\"], link2, mat2) \n", 328 | " settransform!(vis[\"link1\",\"link2\"], MeshCat.Translation(0,0,l1))\n", 329 | " settransform!(vis[\"link1\",\"joint\"], MeshCat.Translation(0,0,l1))\n", 330 | "end\n", 331 | "\n", 332 | "function RotX(alpha)\n", 333 | " c, s = cos(alpha), sin(alpha)\n", 334 | " [1 0 0; 0 c -s; 0 s c]\n", 335 | "end\n", 336 | "function update_acro_pose!(vis, x)\n", 337 | " l1, l2 = [1, 1.]\n", 338 | " settransform!(vis[\"robot\",\"link1\"], MeshCat.LinearMap(RotX(x[1]-pi/2)))\n", 339 | " settransform!(vis[\"robot\",\"link1\",\"link2\"], MeshCat.compose(MeshCat.Translation(0,0,l1), MeshCat.LinearMap(RotX(x[2]))))\n", 340 | "end" 341 | ] 342 | }, 343 | { 344 | "cell_type": "code", 345 | "execution_count": null, 346 | "metadata": {}, 347 | "outputs": [], 348 | "source": [ 349 | "vis = Visualizer()\n", 350 | "build_acrobot!(vis[\"robot\"])\n", 351 | "anim = MeshCat.Animation(floor(Int, 1 / h))\n", 352 | "for k = 1:Nt\n", 353 | " MeshCat.atframe(anim, k) do\n", 354 | " update_acro_pose!(vis, xtraj[:,k])\n", 355 | " end\n", 356 | "end\n", 357 | "MeshCat.setanimation!(vis, anim)\n", 358 | "render(vis)" 359 | ] 360 | }, 361 | { 362 | "cell_type": "code", 363 | "execution_count": null, 364 | "metadata": {}, 365 | "outputs": [], 366 | "source": [] 367 | } 368 | ], 369 | "metadata": { 370 | "kernelspec": { 371 | "display_name": "Julia 1.6.7", 372 | "language": "julia", 373 | "name": "julia-1.6" 374 | }, 375 | "language_info": { 376 | "file_extension": ".jl", 377 | "mimetype": "application/julia", 378 | "name": "julia", 379 | "version": "1.6.7" 380 | } 381 | }, 382 | "nbformat": 4, 383 | "nbformat_minor": 4 384 | } 385 | -------------------------------------------------------------------------------- /Lecture 11/Lecture 11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 11/Lecture 11.pdf -------------------------------------------------------------------------------- /Lecture 11/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" 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 | -------------------------------------------------------------------------------- /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 = RobotZoo.dynamics(a, x, u)\n", 47 | " f2 = RobotZoo.dynamics(a, x + 0.5*h*f1, u)\n", 48 | " f3 = RobotZoo.dynamics(a, x + 0.5*h*f2, u)\n", 49 | " f4 = RobotZoo.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.1;\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 | "# Acrobot (doublependulum)\n", 350 | "import MeshCat as mc\n", 351 | "using Colors\n", 352 | "\n", 353 | "function RotX(alpha)\n", 354 | " c, s = cos(alpha), sin(alpha)\n", 355 | " [1 0 0; 0 c -s; 0 s c]\n", 356 | "end\n", 357 | "\n", 358 | "function create_acrobot!(vis, color=colorant\"blue\", thick=0.05)\n", 359 | " l1,l2 = [1.,1.]\n", 360 | " hinge = mc.Cylinder(mc.Point(-0.05,0,0), mc.Point(0.05,0,0), 0.05)\n", 361 | " dim1 = mc.Vec(thick, thick, l1)\n", 362 | " link1 = mc.HyperRectangle(mc.Vec(-thick/2,-thick/2,0),dim1)\n", 363 | " dim2 = mc.Vec(thick, thick, l2)\n", 364 | " link2 = mc.HyperRectangle(mc.Vec(-thick/2,-thick/2,0),dim2)\n", 365 | " mat1 = mc.MeshPhongMaterial(color=colorant\"grey\")\n", 366 | " mat2 = mc.MeshPhongMaterial(color=color)\n", 367 | " mc.setobject!(vis[\"base\"], hinge, mat1) \n", 368 | " mc.setobject!(vis[\"link1\"], link1, mat2) \n", 369 | " mc.setobject!(vis[\"link1\",\"joint\"], hinge, mat1) \n", 370 | " mc.setobject!(vis[\"link1\",\"link2\"], link2, mat2) \n", 371 | " mc.settransform!(vis[\"link1\",\"link2\"], mc.Translation(0,0,l1))\n", 372 | " mc.settransform!(vis[\"link1\",\"joint\"], mc.Translation(0,0,l1))\n", 373 | "end\n", 374 | "\n", 375 | "function update_acro_pose!(vis, x)\n", 376 | " l1, l2 = [1, 1.]\n", 377 | " mc.settransform!(vis[\"robot\",\"link1\"], mc.LinearMap(RotX(x[1]-pi/2)))\n", 378 | " mc.settransform!(vis[\"robot\",\"link1\",\"link2\"], mc.compose(mc.Translation(0,0,l1), mc.LinearMap(RotX(x[2]))))\n", 379 | "end\n", 380 | "\n", 381 | "function animate_acrobot(X, dt)\n", 382 | " vis = mc.Visualizer()\n", 383 | " create_acrobot!(vis[\"robot\"])\n", 384 | " anim = mc.Animation(floor(Int,1/dt))\n", 385 | " for k = 1:length(X)\n", 386 | " mc.atframe(anim, k) do\n", 387 | " update_acro_pose!(vis,X[k])\n", 388 | " end\n", 389 | " end\n", 390 | " mc.setanimation!(vis, anim)\n", 391 | " return mc.render(vis)\n", 392 | "end" 393 | ] 394 | }, 395 | { 396 | "cell_type": "code", 397 | "execution_count": null, 398 | "metadata": {}, 399 | "outputs": [], 400 | "source": [ 401 | "X1 = [Vector(x) for x in eachcol(xtraj)];\n", 402 | "animate_acrobot(X1, h)" 403 | ] 404 | }, 405 | { 406 | "cell_type": "code", 407 | "execution_count": null, 408 | "metadata": {}, 409 | "outputs": [], 410 | "source": [] 411 | } 412 | ], 413 | "metadata": { 414 | "kernelspec": { 415 | "display_name": "Julia 1.6.7", 416 | "language": "julia", 417 | "name": "julia-1.6" 418 | }, 419 | "language_info": { 420 | "file_extension": ".jl", 421 | "mimetype": "application/julia", 422 | "name": "julia", 423 | "version": "1.6.7" 424 | } 425 | }, 426 | "nbformat": 4, 427 | "nbformat_minor": 4 428 | } 429 | -------------------------------------------------------------------------------- /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 = RobotZoo.dynamics(a, x, u)\n", 46 | " f2 = RobotZoo.dynamics(a, x + 0.5*h*f1, u)\n", 47 | " f3 = RobotZoo.dynamics(a, x + 0.5*h*f2, u)\n", 48 | " f4 = RobotZoo.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 | "source": [ 160 | "function backward_pass!(p,P,d,K)\n", 161 | " \n", 162 | " ΔJ = 0.0\n", 163 | " p[:,Nt] .= Qn*(xtraj[:,Nt]-xgoal)\n", 164 | " P[:,:,Nt] .= Qn\n", 165 | " \n", 166 | " for k = (Nt-1):-1:1\n", 167 | " #Calculate derivatives\n", 168 | " q = Q*(xtraj[:,k]-xgoal)\n", 169 | " r = R*utraj[k]\n", 170 | " \n", 171 | " A = dfdx(xtraj[:,k], utraj[k]) #ForwardDiff.jacobian(dx->dynamics_rk4(dx,utraj[k]),xtraj[:,k])\n", 172 | " B = dfdu(xtraj[:,k], utraj[k]) #ForwardDiff.derivative(du->dynamics_rk4(xtraj[:,k],du),utraj[k])\n", 173 | " \n", 174 | " Ax = dAdx(xtraj[:,k], utraj[k])\n", 175 | " Bx = dBdx(xtraj[:,k], utraj[k])\n", 176 | " Au = dAdu(xtraj[:,k], utraj[k])\n", 177 | " Bu = dBdu(xtraj[:,k], utraj[k])\n", 178 | " \n", 179 | " gx = q + A'*p[:,k+1]\n", 180 | " gu = r + B'*p[:,k+1]\n", 181 | " \n", 182 | " #iLQR (Gauss-Newton) version\n", 183 | " Gxx = Q + A'*P[:,:,k+1]*A\n", 184 | " Guu = R + B'*P[:,:,k+1]*B\n", 185 | " Gxu = A'*P[:,:,k+1]*B\n", 186 | " Gux = B'*P[:,:,k+1]*A\n", 187 | " \n", 188 | " #DDP (full Newton) version\n", 189 | " #Gxx = Q + A'*P[:,:,k+1]*A + kron(p[:,k+1]',I(Nx))*comm(Nx,Nx)*Ax\n", 190 | " #Guu = R + B'*P[:,:,k+1]*B + (kron(p[:,k+1]',I(Nu))*comm(Nx,Nu)*Bu)[1]\n", 191 | " #Gxu = A'*P[:,:,k+1]*B + kron(p[:,k+1]',I(Nx))*comm(Nx,Nx)*Au\n", 192 | " #Gux = B'*P[:,:,k+1]*A + kron(p[:,k+1]',I(Nu))*comm(Nx,Nu)*Bx\n", 193 | " \n", 194 | " β = 0.1\n", 195 | " while !isposdef(Symmetric([Gxx Gxu; Gux Guu]))\n", 196 | " Gxx += A'*β*I*A\n", 197 | " Guu += B'*β*I*B\n", 198 | " Gxu += A'*β*I*B\n", 199 | " Gux += B'*β*I*A\n", 200 | " β = 2*β\n", 201 | " display(\"regularizing G\")\n", 202 | " #display(β)\n", 203 | " end\n", 204 | " \n", 205 | " d[k] = Guu\\gu\n", 206 | " K[:,:,k] .= Guu\\Gux\n", 207 | " \n", 208 | " p[:,k] .= dropdims(gx - K[:,:,k]'*gu + K[:,:,k]'*Guu*d[k] - Gxu*d[k], dims=2)\n", 209 | " P[:,:,k] .= Gxx + K[:,:,k]'*Guu*K[:,:,k] - Gxu*K[:,:,k] - K[:,:,k]'*Gux\n", 210 | " \n", 211 | " ΔJ += gu'*d[k]\n", 212 | " end\n", 213 | " \n", 214 | " return ΔJ\n", 215 | "end" 216 | ] 217 | }, 218 | { 219 | "cell_type": "code", 220 | "execution_count": null, 221 | "metadata": {}, 222 | "outputs": [], 223 | "source": [ 224 | "#Initial guess\n", 225 | "x0 = [0; 0; 0; 0]\n", 226 | "xgoal = [0, pi/2, 0, 0]\n", 227 | "xtraj = kron(ones(1,Nt), x0)\n", 228 | "utraj = 0.001*ones(Nt-1);" 229 | ] 230 | }, 231 | { 232 | "cell_type": "code", 233 | "execution_count": null, 234 | "metadata": {}, 235 | "outputs": [], 236 | "source": [ 237 | "#Initial Rollout\n", 238 | "for k = 1:(Nt-1)\n", 239 | " xtraj[:,k+1] .= dynamics_rk4(xtraj[:,k],utraj[k])\n", 240 | "end\n", 241 | "J = cost(xtraj,utraj)" 242 | ] 243 | }, 244 | { 245 | "cell_type": "code", 246 | "execution_count": null, 247 | "metadata": {}, 248 | "outputs": [], 249 | "source": [ 250 | "#DDP Algorithm\n", 251 | "p = ones(Nx,Nt)\n", 252 | "P = zeros(Nx,Nx,Nt)\n", 253 | "d = ones(Nt-1)\n", 254 | "K = zeros(Nu,Nx,Nt-1)\n", 255 | "ΔJ = 1.0\n", 256 | "\n", 257 | "xn = zeros(Nx,Nt)\n", 258 | "un = zeros(Nt-1)\n", 259 | "\n", 260 | "gx = zeros(Nx)\n", 261 | "gu = 0.0\n", 262 | "Gxx = zeros(Nx,Nx)\n", 263 | "Guu = 0.0\n", 264 | "Gxu = zeros(Nx)\n", 265 | "Gux = zeros(Nx)\n", 266 | "\n", 267 | "iter = 0\n", 268 | "while maximum(abs.(d[:])) > 1e-3\n", 269 | " iter += 1 \n", 270 | " \n", 271 | " #Backward Pass\n", 272 | " ΔJ = backward_pass!(p,P,d,K)\n", 273 | "\n", 274 | " #Forward rollout with line search\n", 275 | " xn[:,1] = xtraj[:,1]\n", 276 | " α = 1.0\n", 277 | "\n", 278 | " for k = 1:(Nt-1)\n", 279 | " un[k] = utraj[k] - α*d[k] - dot(K[:,:,k],xn[:,k]-xtraj[:,k])\n", 280 | " xn[:,k+1] .= dynamics_rk4(xn[:,k],un[k])\n", 281 | " end\n", 282 | " Jn = cost(xn,un)\n", 283 | " \n", 284 | " while isnan(Jn) || Jn > (J - 1e-2*α*ΔJ)\n", 285 | " α = 0.5*α\n", 286 | " for k = 1:(Nt-1)\n", 287 | " un[k] = utraj[k] - α*d[k] - dot(K[:,:,k],xn[:,k]-xtraj[:,k])\n", 288 | " xn[:,k+1] .= dynamics_rk4(xn[:,k],un[k])\n", 289 | " end\n", 290 | " Jn = cost(xn,un)\n", 291 | " end\n", 292 | " \n", 293 | " J = Jn\n", 294 | " xtraj .= xn\n", 295 | " utraj .= un\n", 296 | "end" 297 | ] 298 | }, 299 | { 300 | "cell_type": "code", 301 | "execution_count": null, 302 | "metadata": {}, 303 | "outputs": [], 304 | "source": [ 305 | "iter" 306 | ] 307 | }, 308 | { 309 | "cell_type": "code", 310 | "execution_count": null, 311 | "metadata": {}, 312 | "outputs": [], 313 | "source": [ 314 | "J" 315 | ] 316 | }, 317 | { 318 | "cell_type": "code", 319 | "execution_count": null, 320 | "metadata": {}, 321 | "outputs": [], 322 | "source": [ 323 | "plot(thist,xtraj[1,:])\n", 324 | "plot(thist,xtraj[2,:])" 325 | ] 326 | }, 327 | { 328 | "cell_type": "code", 329 | "execution_count": null, 330 | "metadata": {}, 331 | "outputs": [], 332 | "source": [ 333 | "plot(thist[1:Nt-1],utraj)" 334 | ] 335 | }, 336 | { 337 | "cell_type": "code", 338 | "execution_count": null, 339 | "metadata": {}, 340 | "outputs": [], 341 | "source": [ 342 | "import MeshCat as mc \n", 343 | "\n", 344 | "function rotx(θ)\n", 345 | " s, c = sincos(θ)\n", 346 | " return [1 0 0; 0 c -s; 0 s c]\n", 347 | "end\n", 348 | "function create_cartpole!(vis)\n", 349 | " mc.setobject!(vis[:cart], mc.HyperRectangle(mc.Vec(-.25,-1.0,-.15), mc.Vec(0.5,2,0.3)))\n", 350 | " mc.setobject!(vis[:pole], mc.Cylinder(mc.Point(0,0,-.75), mc.Point(0,0,.75), 0.05))\n", 351 | " mc.setobject!(vis[:a], mc.HyperSphere(mc.Point(0,0,0.0),0.1))\n", 352 | " mc.setobject!(vis[:b], mc.HyperSphere(mc.Point(0,0,0.0),0.1))\n", 353 | "end\n", 354 | "function update_cartpole_transform!(vis,x)\n", 355 | " pole_o = 0.3\n", 356 | " px = x[1]\n", 357 | " θ = x[2]\n", 358 | " mc.settransform!(vis[:cart], mc.Translation([0,px,0.0]))\n", 359 | " p1 = [pole_o,px,0]\n", 360 | " p2 = p1 + 1.5*[0,sin(θ), -cos(θ)]\n", 361 | " mc.settransform!(vis[:a], mc.Translation(p1))\n", 362 | " mc.settransform!(vis[:b], mc.Translation(p2))\n", 363 | " mc.settransform!(vis[:pole], mc.Translation(0.5*(p1 + p2)) ∘ mc.LinearMap(rotx(θ))) \n", 364 | "end\n", 365 | "\n", 366 | "function animate_cartpole(X, dt)\n", 367 | " vis = mc.Visualizer()\n", 368 | " create_cartpole!(vis)\n", 369 | " anim = mc.Animation(floor(Int,1/dt))\n", 370 | " for k = 1:length(X)\n", 371 | " mc.atframe(anim, k) do\n", 372 | " update_cartpole_transform!(vis,X[k])\n", 373 | " end\n", 374 | " end\n", 375 | " mc.setanimation!(vis, anim)\n", 376 | " return mc.render(vis)\n", 377 | "end" 378 | ] 379 | }, 380 | { 381 | "cell_type": "code", 382 | "execution_count": null, 383 | "metadata": {}, 384 | "outputs": [], 385 | "source": [ 386 | "X1 = [Vector(x) for x in eachcol(xtraj)];\n", 387 | "animate_cartpole(X1, h)" 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 | -------------------------------------------------------------------------------- /Lecture 11/guess.jld2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 11/guess.jld2 -------------------------------------------------------------------------------- /Lecture 12/Lecture 12.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 12/Lecture 12.pdf -------------------------------------------------------------------------------- /Lecture 12/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" 3 | Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" 4 | MathOptInterface = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" 5 | MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11" 6 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 7 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 8 | -------------------------------------------------------------------------------- /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 | "# Acrobot (doublependulum)\n", 373 | "import MeshCat as mc\n", 374 | "using Colors\n", 375 | "\n", 376 | "function RotX(alpha)\n", 377 | " c, s = cos(alpha), sin(alpha)\n", 378 | " [1 0 0; 0 c -s; 0 s c]\n", 379 | "end\n", 380 | "\n", 381 | "function create_acrobot!(vis, color=colorant\"blue\", thick=0.05)\n", 382 | " l1,l2 = [1.,1.]\n", 383 | " hinge = mc.Cylinder(mc.Point(-0.05,0,0), mc.Point(0.05,0,0), 0.05)\n", 384 | " dim1 = mc.Vec(thick, thick, l1)\n", 385 | " link1 = mc.HyperRectangle(mc.Vec(-thick/2,-thick/2,0),dim1)\n", 386 | " dim2 = mc.Vec(thick, thick, l2)\n", 387 | " link2 = mc.HyperRectangle(mc.Vec(-thick/2,-thick/2,0),dim2)\n", 388 | " mat1 = mc.MeshPhongMaterial(color=colorant\"grey\")\n", 389 | " mat2 = mc.MeshPhongMaterial(color=color)\n", 390 | " mc.setobject!(vis[\"base\"], hinge, mat1) \n", 391 | " mc.setobject!(vis[\"link1\"], link1, mat2) \n", 392 | " mc.setobject!(vis[\"link1\",\"joint\"], hinge, mat1) \n", 393 | " mc.setobject!(vis[\"link1\",\"link2\"], link2, mat2) \n", 394 | " mc.settransform!(vis[\"link1\",\"link2\"], mc.Translation(0,0,l1))\n", 395 | " mc.settransform!(vis[\"link1\",\"joint\"], mc.Translation(0,0,l1))\n", 396 | "end\n", 397 | "\n", 398 | "function update_acro_pose!(vis, x)\n", 399 | " l1, l2 = [1, 1.]\n", 400 | " mc.settransform!(vis[\"robot\",\"link1\"], mc.LinearMap(RotX(x[1]-pi/2)))\n", 401 | " mc.settransform!(vis[\"robot\",\"link1\",\"link2\"], mc.compose(mc.Translation(0,0,l1), mc.LinearMap(RotX(x[2]))))\n", 402 | "end\n", 403 | "\n", 404 | "function animate_acrobot(X, dt)\n", 405 | " vis = mc.Visualizer()\n", 406 | " create_acrobot!(vis[\"robot\"])\n", 407 | " anim = mc.Animation(floor(Int,1/dt))\n", 408 | " for k = 1:length(X)\n", 409 | " mc.atframe(anim, k) do\n", 410 | " update_acro_pose!(vis,X[k])\n", 411 | " end\n", 412 | " end\n", 413 | " mc.setanimation!(vis, anim)\n", 414 | " return mc.render(vis)\n", 415 | "end" 416 | ] 417 | }, 418 | { 419 | "cell_type": "code", 420 | "execution_count": null, 421 | "metadata": {}, 422 | "outputs": [], 423 | "source": [ 424 | "X1 = [Vector(x) for x in eachcol(xtraj)];\n", 425 | "animate_acrobot(X1, h)" 426 | ] 427 | }, 428 | { 429 | "cell_type": "code", 430 | "execution_count": null, 431 | "metadata": {}, 432 | "outputs": [], 433 | "source": [] 434 | } 435 | ], 436 | "metadata": { 437 | "kernelspec": { 438 | "display_name": "Julia 1.6.7", 439 | "language": "julia", 440 | "name": "julia-1.6" 441 | }, 442 | "language_info": { 443 | "file_extension": ".jl", 444 | "mimetype": "application/julia", 445 | "name": "julia", 446 | "version": "1.6.7" 447 | } 448 | }, 449 | "nbformat": 4, 450 | "nbformat_minor": 4 451 | } 452 | -------------------------------------------------------------------------------- /Lecture 13/Lecture 13.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 13/Lecture 13.pdf -------------------------------------------------------------------------------- /Lecture 13/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 13/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 13/Project.toml -------------------------------------------------------------------------------- /Lecture 13/rbsim.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "852cc810-f6dc-400d-b9e3-be8274023446", 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": "3b4881e0-e594-4800-b9b2-9ce861d9c0cb", 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "using LinearAlgebra\n", 21 | "using ForwardDiff" 22 | ] 23 | }, 24 | { 25 | "cell_type": "code", 26 | "execution_count": null, 27 | "id": "e5fcb4bf-a045-4910-8532-4ccce3dfd0bc", 28 | "metadata": {}, 29 | "outputs": [], 30 | "source": [ 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" 36 | ] 37 | }, 38 | { 39 | "cell_type": "code", 40 | "execution_count": null, 41 | "id": "df940483-86ce-4cef-ae3f-e3f67f3e3ae5", 42 | "metadata": {}, 43 | "outputs": [], 44 | "source": [ 45 | "function L(q)\n", 46 | " s = q[1]\n", 47 | " v = q[2:4]\n", 48 | " L = [s -v';\n", 49 | " v s*I+hat(v)]\n", 50 | " return L\n", 51 | "end" 52 | ] 53 | }, 54 | { 55 | "cell_type": "code", 56 | "execution_count": null, 57 | "id": "f369ce00-cd8e-4170-8918-acf5aad604b2", 58 | "metadata": {}, 59 | "outputs": [], 60 | "source": [ 61 | "function R(q)\n", 62 | " s = q[1]\n", 63 | " v = q[2:4]\n", 64 | " R = [s -v';\n", 65 | " v s*I-hat(v)]\n", 66 | " return R\n", 67 | "end" 68 | ] 69 | }, 70 | { 71 | "cell_type": "code", 72 | "execution_count": null, 73 | "id": "37243558-83b9-4b33-b5d9-21f3468a6c58", 74 | "metadata": {}, 75 | "outputs": [], 76 | "source": [ 77 | "T = Diagonal([1; -ones(3)])\n", 78 | "H = [zeros(1,3); I];" 79 | ] 80 | }, 81 | { 82 | "cell_type": "code", 83 | "execution_count": null, 84 | "id": "415243be-a32b-48c9-b052-254c1da2a193", 85 | "metadata": {}, 86 | "outputs": [], 87 | "source": [ 88 | "function G(q)\n", 89 | " G = L(q)*H\n", 90 | "end\n", 91 | "\n", 92 | "function Q(q)\n", 93 | " return H'*(R(q)'*L(q))*H\n", 94 | "end" 95 | ] 96 | }, 97 | { 98 | "cell_type": "code", 99 | "execution_count": null, 100 | "id": "468693dd-f4be-4b27-b4aa-d1d96a8e10a6", 101 | "metadata": {}, 102 | "outputs": [], 103 | "source": [ 104 | "J = Diagonal([1; 2; 3])\n", 105 | "h = 0.1" 106 | ] 107 | }, 108 | { 109 | "cell_type": "code", 110 | "execution_count": null, 111 | "id": "0370d040-1802-4137-ac5e-c3a734fbac1b", 112 | "metadata": {}, 113 | "outputs": [], 114 | "source": [ 115 | "#initial conditions\n", 116 | "Q0 = Array(I(3))\n", 117 | "q0 = [1; 0; 0; 0]\n", 118 | "ω0 = randn(3)\n", 119 | "x0 = [vec(Q0); ω0]\n", 120 | "x0q = [q0; ω0]" 121 | ] 122 | }, 123 | { 124 | "cell_type": "code", 125 | "execution_count": null, 126 | "id": "a8c05162-9c79-416c-b36d-d199a1da9684", 127 | "metadata": {}, 128 | "outputs": [], 129 | "source": [ 130 | "#dynamics\n", 131 | "function dynamics(x)\n", 132 | " Q = reshape(x[1:9],3,3)\n", 133 | " ω = x[10:12]\n", 134 | " \n", 135 | " Q̇ = Q*hat(ω)\n", 136 | " ω̇ = -J\\(hat(ω)*J*ω)\n", 137 | "\n", 138 | " ẋ = [vec(Q̇); ω̇]\n", 139 | "end" 140 | ] 141 | }, 142 | { 143 | "cell_type": "code", 144 | "execution_count": null, 145 | "id": "a8ef3f75-5412-454b-91d1-90db2ec336e7", 146 | "metadata": {}, 147 | "outputs": [], 148 | "source": [ 149 | "function rkstep(x)\n", 150 | " f1 = dynamics(x)\n", 151 | " f2 = dynamics(x + 0.5*h*f1)\n", 152 | " f3 = dynamics(x + 0.5*h*f2)\n", 153 | " f4 = dynamics(x + h*f3)\n", 154 | " xn = x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 155 | " return xn\n", 156 | "end" 157 | ] 158 | }, 159 | { 160 | "cell_type": "code", 161 | "execution_count": null, 162 | "id": "d8a8b470-3cff-4a5e-9094-81494d9bf84b", 163 | "metadata": {}, 164 | "outputs": [], 165 | "source": [ 166 | "xk = x0\n", 167 | "for k = 1:10000\n", 168 | " xk = rkstep(xk)\n", 169 | "end" 170 | ] 171 | }, 172 | { 173 | "cell_type": "code", 174 | "execution_count": null, 175 | "id": "6190591d-839b-411c-9217-793aac0e507e", 176 | "metadata": {}, 177 | "outputs": [], 178 | "source": [ 179 | "Qk = reshape(xk[1:9],3,3)" 180 | ] 181 | }, 182 | { 183 | "cell_type": "code", 184 | "execution_count": null, 185 | "id": "e6ab480e-05a4-4ef9-81cc-cfe74618fbbc", 186 | "metadata": {}, 187 | "outputs": [], 188 | "source": [ 189 | "Qk'*Qk" 190 | ] 191 | }, 192 | { 193 | "cell_type": "code", 194 | "execution_count": null, 195 | "id": "8eaa0035-4020-4240-8215-9663b390cc7d", 196 | "metadata": {}, 197 | "outputs": [], 198 | "source": [ 199 | "#quaternion dynamics\n", 200 | "function qdynamics(x)\n", 201 | " q = x[1:4]\n", 202 | " ω = x[5:7]\n", 203 | " \n", 204 | " q̇ = 0.5*L(q)*H*ω\n", 205 | " ω̇ = -J\\(hat(ω)*J*ω)\n", 206 | "\n", 207 | " ẋ = [q̇; ω̇]\n", 208 | "end" 209 | ] 210 | }, 211 | { 212 | "cell_type": "code", 213 | "execution_count": null, 214 | "id": "d0b295da-c058-43a1-805f-3703da5545a7", 215 | "metadata": {}, 216 | "outputs": [], 217 | "source": [ 218 | "function qrkstep(x)\n", 219 | " f1 = qdynamics(x)\n", 220 | " f2 = qdynamics(x + 0.5*h*f1)\n", 221 | " f3 = qdynamics(x + 0.5*h*f2)\n", 222 | " f4 = qdynamics(x + h*f3)\n", 223 | " xn = x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 224 | " xn[1:4] .= xn[1:4]./norm(xn[1:4])\n", 225 | " return xn\n", 226 | "end" 227 | ] 228 | }, 229 | { 230 | "cell_type": "code", 231 | "execution_count": null, 232 | "id": "c2891453-84e3-41ec-8dc6-18bdf571a998", 233 | "metadata": {}, 234 | "outputs": [], 235 | "source": [ 236 | "xkq = x0q\n", 237 | "for k = 1:10000\n", 238 | " xkq = qrkstep(xkq)\n", 239 | "end" 240 | ] 241 | }, 242 | { 243 | "cell_type": "code", 244 | "execution_count": null, 245 | "id": "8bd11529-0930-414a-a0b5-63ac3b7e4e21", 246 | "metadata": {}, 247 | "outputs": [], 248 | "source": [ 249 | "qk = xkq[1:4]" 250 | ] 251 | }, 252 | { 253 | "cell_type": "code", 254 | "execution_count": null, 255 | "id": "5381338f-4c0c-40ae-a24a-4b4e682dcec2", 256 | "metadata": {}, 257 | "outputs": [], 258 | "source": [ 259 | "norm(qk)" 260 | ] 261 | }, 262 | { 263 | "cell_type": "code", 264 | "execution_count": null, 265 | "id": "868e6bbc-18b4-4f89-8494-aa84e2d44649", 266 | "metadata": {}, 267 | "outputs": [], 268 | "source": [] 269 | } 270 | ], 271 | "metadata": { 272 | "kernelspec": { 273 | "display_name": "Julia 1.6.7", 274 | "language": "julia", 275 | "name": "julia-1.6" 276 | }, 277 | "language_info": { 278 | "file_extension": ".jl", 279 | "mimetype": "application/julia", 280 | "name": "julia", 281 | "version": "1.6.7" 282 | } 283 | }, 284 | "nbformat": 4, 285 | "nbformat_minor": 5 286 | } 287 | -------------------------------------------------------------------------------- /Lecture 14/Lecture 14.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/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-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/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 | "q-qtrue" 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 | } 187 | ], 188 | "metadata": { 189 | "kernelspec": { 190 | "display_name": "Julia 1.6.7", 191 | "language": "julia", 192 | "name": "julia-1.6" 193 | }, 194 | "language_info": { 195 | "file_extension": ".jl", 196 | "mimetype": "application/julia", 197 | "name": "julia", 198 | "version": "1.6.7" 199 | } 200 | }, 201 | "nbformat": 4, 202 | "nbformat_minor": 4 203 | } 204 | -------------------------------------------------------------------------------- /Lecture 15/Lecture 15.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/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 | TrajOptPlots = "7770976a-8dee-4930-bf39-a1782fd21ce6" 7 | -------------------------------------------------------------------------------- /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-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/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 | MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11" 6 | Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" 7 | -------------------------------------------------------------------------------- /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 | "metadata": { 146 | "kernelspec": { 147 | "display_name": "Julia 1.6.7", 148 | "language": "julia", 149 | "name": "julia-1.6" 150 | }, 151 | "language_info": { 152 | "file_extension": ".jl", 153 | "mimetype": "application/julia", 154 | "name": "julia", 155 | "version": "1.6.7" 156 | } 157 | }, 158 | "nbformat": 4, 159 | "nbformat_minor": 4 160 | } 161 | -------------------------------------------------------------------------------- /Lecture 17/Lecture 17.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 17/Lecture 17.pdf -------------------------------------------------------------------------------- /Lecture 17/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 = "cde29ddf7e5726c9fb511f340244ea3481267608" 6 | uuid = "79e6a3ab-5dfb-504d-930d-738a2a938a0e" 7 | version = "3.7.2" 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", "SparseArrays", "SuiteSparse"] 20 | git-tree-sha1 = "16267cf279190ca7c1b30d020758ced95db89cd0" 21 | uuid = "4fba245c-0d91-5ea0-9b3e-6abc04ee57a9" 22 | version = "7.5.1" 23 | 24 | [[Artifacts]] 25 | uuid = "56f22d72-fd6d-98f1-02f0-08ddc0907c33" 26 | 27 | [[AxisAlgorithms]] 28 | deps = ["LinearAlgebra", "Random", "SparseArrays", "WoodburyMatrices"] 29 | git-tree-sha1 = "66771c8d21c8ff5e3a93379480a2307ac36863f7" 30 | uuid = "13072b0f-2c55-5437-9ae7-d433b7a33950" 31 | version = "1.0.1" 32 | 33 | [[Base64]] 34 | uuid = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f" 35 | 36 | [[BenchmarkTools]] 37 | deps = ["JSON", "Logging", "Printf", "Profile", "Statistics", "UUIDs"] 38 | git-tree-sha1 = "f1dff6729bc61f4d49e140da1af55dcd1ac97b2f" 39 | uuid = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf" 40 | version = "1.5.0" 41 | 42 | [[BitTwiddlingConvenienceFunctions]] 43 | deps = ["Static"] 44 | git-tree-sha1 = "0c5f81f47bbbcf4aea7b2959135713459170798b" 45 | uuid = "62783981-4cbd-42fc-bca8-16325de8dc4b" 46 | version = "0.1.5" 47 | 48 | [[CPUSummary]] 49 | deps = ["CpuId", "IfElse", "PrecompileTools", "Static"] 50 | git-tree-sha1 = "601f7e7b3d36f18790e2caf83a882d88e9b71ff1" 51 | uuid = "2a0fbf3d-bb9c-48f3-b0a9-814d99fd7ab9" 52 | version = "0.2.4" 53 | 54 | [[Calculus]] 55 | deps = ["LinearAlgebra"] 56 | git-tree-sha1 = "f641eb0a4f00c343bbc32346e1217b86f3ce9dad" 57 | uuid = "49dc2e85-a5d0-5ad3-a950-438e2897f1b9" 58 | version = "0.5.1" 59 | 60 | [[ChainRulesCore]] 61 | deps = ["Compat", "LinearAlgebra", "SparseArrays"] 62 | git-tree-sha1 = "575cd02e080939a33b6df6c5853d14924c08e35b" 63 | uuid = "d360d2e6-b24c-11e9-a2a3-2a2ae2dbcce4" 64 | version = "1.23.0" 65 | 66 | [[ChangesOfVariables]] 67 | deps = ["InverseFunctions", "LinearAlgebra", "Test"] 68 | git-tree-sha1 = "2fba81a302a7be671aefe194f0525ef231104e7f" 69 | uuid = "9e997f8a-9a97-42d5-a9f1-ce6bfc15e2c0" 70 | version = "0.1.8" 71 | 72 | [[CloseOpenIntervals]] 73 | deps = ["Static", "StaticArrayInterface"] 74 | git-tree-sha1 = "70232f82ffaab9dc52585e0dd043b5e0c6b714f1" 75 | uuid = "fb6a15b2-703c-40df-9091-08a04967cfa9" 76 | version = "0.1.12" 77 | 78 | [[CommonSubexpressions]] 79 | deps = ["MacroTools", "Test"] 80 | git-tree-sha1 = "7b8a93dba8af7e3b42fecabf646260105ac373f7" 81 | uuid = "bbf7d656-a473-5ed7-a52c-81e309532950" 82 | version = "0.3.0" 83 | 84 | [[Compat]] 85 | deps = ["Dates", "LinearAlgebra", "TOML", "UUIDs"] 86 | git-tree-sha1 = "c955881e3c981181362ae4088b35995446298b80" 87 | uuid = "34da2185-b29b-5c13-b0c7-acf172513d20" 88 | version = "4.14.0" 89 | 90 | [[CompilerSupportLibraries_jll]] 91 | deps = ["Artifacts", "Libdl"] 92 | uuid = "e66e0078-7015-5450-92f7-15fbd957f2ae" 93 | 94 | [[ConstructionBase]] 95 | deps = ["LinearAlgebra"] 96 | git-tree-sha1 = "260fd2400ed2dab602a7c15cf10c1933c59930a2" 97 | uuid = "187b0558-2788-49d3-abe0-74a17ed4e7c9" 98 | version = "1.5.5" 99 | 100 | [[CpuId]] 101 | deps = ["Markdown"] 102 | git-tree-sha1 = "32d125af0fb8ec3f8935896122c5e345709909e5" 103 | uuid = "adafc99b-e345-5852-983c-f28acb93d879" 104 | version = "0.3.0" 105 | 106 | [[Crayons]] 107 | git-tree-sha1 = "249fe38abf76d48563e2f4556bebd215aa317e15" 108 | uuid = "a8cc5b0e-0ffa-5ad4-8c14-923d3ee1735f" 109 | version = "4.1.1" 110 | 111 | [[Dates]] 112 | deps = ["Printf"] 113 | uuid = "ade2ca70-3891-5945-98fb-dc099432e06a" 114 | 115 | [[DiffResults]] 116 | deps = ["StaticArraysCore"] 117 | git-tree-sha1 = "782dd5f4561f5d267313f23853baaaa4c52ea621" 118 | uuid = "163ba53b-c6d8-5494-b064-1a9d43ac40c5" 119 | version = "1.1.0" 120 | 121 | [[DiffRules]] 122 | deps = ["IrrationalConstants", "LogExpFunctions", "NaNMath", "Random", "SpecialFunctions"] 123 | git-tree-sha1 = "23163d55f885173722d1e4cf0f6110cdbaf7e272" 124 | uuid = "b552c78f-8df3-52c6-915a-8e097449b14b" 125 | version = "1.15.1" 126 | 127 | [[Distributed]] 128 | deps = ["Random", "Serialization", "Sockets"] 129 | uuid = "8ba89e20-285c-5b6f-9357-94700520ee1b" 130 | 131 | [[DocStringExtensions]] 132 | deps = ["LibGit2"] 133 | git-tree-sha1 = "b19534d1895d702889b219c382a6e18010797f0b" 134 | uuid = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" 135 | version = "0.8.6" 136 | 137 | [[Downloads]] 138 | deps = ["ArgTools", "LibCURL", "NetworkOptions"] 139 | uuid = "f43a241f-c20a-4ad4-852c-f6b1247861c6" 140 | 141 | [[ExprTools]] 142 | git-tree-sha1 = "27415f162e6028e81c72b82ef756bf321213b6ec" 143 | uuid = "e2ba6199-217a-4e67-a87a-7c52f15ade04" 144 | version = "0.1.10" 145 | 146 | [[FiniteDiff]] 147 | deps = ["ArrayInterface", "LinearAlgebra", "Requires", "Setfield", "SparseArrays", "StaticArrays"] 148 | git-tree-sha1 = "73d1214fec245096717847c62d389a5d2ac86504" 149 | uuid = "6a86dc24-6348-571c-b903-95158fe2bd41" 150 | version = "2.22.0" 151 | 152 | [[Formatting]] 153 | deps = ["Logging", "Printf"] 154 | git-tree-sha1 = "fb409abab2caf118986fc597ba84b50cbaf00b87" 155 | uuid = "59287772-0a20-5a39-b81b-1366585eb4c0" 156 | version = "0.4.3" 157 | 158 | [[ForwardDiff]] 159 | deps = ["CommonSubexpressions", "DiffResults", "DiffRules", "LinearAlgebra", "LogExpFunctions", "NaNMath", "Preferences", "Printf", "Random", "SpecialFunctions", "StaticArrays"] 160 | git-tree-sha1 = "cf0fe81336da9fb90944683b8c41984b08793dad" 161 | uuid = "f6369f11-7733-5829-9624-2563aa707210" 162 | version = "0.10.36" 163 | 164 | [[Future]] 165 | deps = ["Random"] 166 | uuid = "9fa8497b-333b-5362-9e8d-4d0656e87820" 167 | 168 | [[HostCPUFeatures]] 169 | deps = ["BitTwiddlingConvenienceFunctions", "IfElse", "Libdl", "Static"] 170 | git-tree-sha1 = "eb8fed28f4994600e29beef49744639d985a04b2" 171 | uuid = "3e5b6fbb-0976-4d2c-9146-d79de83f2fb0" 172 | version = "0.1.16" 173 | 174 | [[HyperDualNumbers]] 175 | deps = ["Calculus", "CommonSubexpressions", "LinearAlgebra", "NaNMath", "SpecialFunctions"] 176 | git-tree-sha1 = "a7e4a62f3428a3bbc0bfd8d06274f95c8af9e60d" 177 | uuid = "50ceba7f-c3ee-5a84-a6e8-3ad40456ec97" 178 | version = "4.0.10" 179 | 180 | [[IfElse]] 181 | git-tree-sha1 = "debdd00ffef04665ccbb3e150747a77560e8fad1" 182 | uuid = "615f187c-cbe4-4ef1-ba3b-2fcf58d6d173" 183 | version = "0.1.1" 184 | 185 | [[InteractiveUtils]] 186 | deps = ["Markdown"] 187 | uuid = "b77e0a4c-d291-57a0-90e8-8db25a27a240" 188 | 189 | [[Interpolations]] 190 | deps = ["AxisAlgorithms", "ChainRulesCore", "LinearAlgebra", "OffsetArrays", "Random", "Ratios", "Requires", "SharedArrays", "SparseArrays", "StaticArrays", "WoodburyMatrices"] 191 | git-tree-sha1 = "b7bc05649af456efc75d178846f47006c2c4c3c7" 192 | uuid = "a98d9a8b-a2ab-59e6-89dd-64a1c18fca59" 193 | version = "0.13.6" 194 | 195 | [[InverseFunctions]] 196 | deps = ["Dates", "Test"] 197 | git-tree-sha1 = "896385798a8d49a255c398bd49162062e4a4c435" 198 | uuid = "3587e190-3f89-42d0-90ee-14403ec27112" 199 | version = "0.1.13" 200 | 201 | [[IrrationalConstants]] 202 | git-tree-sha1 = "630b497eafcc20001bba38a4651b327dcfc491d2" 203 | uuid = "92d709cd-6900-40b7-9082-c6be49f344b6" 204 | version = "0.2.2" 205 | 206 | [[JLLWrappers]] 207 | deps = ["Artifacts", "Preferences"] 208 | git-tree-sha1 = "7e5d6779a1e09a36db2a7b6cff50942a0a7d0fca" 209 | uuid = "692b3bcd-3c85-4b1f-b108-f13ce0eb3210" 210 | version = "1.5.0" 211 | 212 | [[JSON]] 213 | deps = ["Dates", "Mmap", "Parsers", "Unicode"] 214 | git-tree-sha1 = "31e996f0a15c7b280ba9f76636b3ff9e2ae58c9a" 215 | uuid = "682c06a0-de6a-54ab-a142-c8b1cf79cde6" 216 | version = "0.21.4" 217 | 218 | [[LayoutPointers]] 219 | deps = ["ArrayInterface", "LinearAlgebra", "ManualMemory", "SIMDTypes", "Static", "StaticArrayInterface"] 220 | git-tree-sha1 = "62edfee3211981241b57ff1cedf4d74d79519277" 221 | uuid = "10f19ff3-798f-405d-979b-55457f8fc047" 222 | version = "0.1.15" 223 | 224 | [[LibCURL]] 225 | deps = ["LibCURL_jll", "MozillaCACerts_jll"] 226 | uuid = "b27032c2-a3e7-50c8-80cd-2d36dbcbfd21" 227 | 228 | [[LibCURL_jll]] 229 | deps = ["Artifacts", "LibSSH2_jll", "Libdl", "MbedTLS_jll", "Zlib_jll", "nghttp2_jll"] 230 | uuid = "deac9b47-8bc7-5906-a0fe-35ac56dc84c0" 231 | 232 | [[LibGit2]] 233 | deps = ["Base64", "NetworkOptions", "Printf", "SHA"] 234 | uuid = "76f85450-5226-5b5a-8eaa-529ad045b433" 235 | 236 | [[LibSSH2_jll]] 237 | deps = ["Artifacts", "Libdl", "MbedTLS_jll"] 238 | uuid = "29816b5a-b9ab-546f-933c-edad1886dfa8" 239 | 240 | [[Libdl]] 241 | uuid = "8f399da3-3557-5675-b5ff-fb832c97cbdb" 242 | 243 | [[LinearAlgebra]] 244 | deps = ["Libdl"] 245 | uuid = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" 246 | 247 | [[LogExpFunctions]] 248 | deps = ["ChainRulesCore", "ChangesOfVariables", "DocStringExtensions", "InverseFunctions", "IrrationalConstants", "LinearAlgebra"] 249 | git-tree-sha1 = "18144f3e9cbe9b15b070288eef858f71b291ce37" 250 | uuid = "2ab3a3ac-af41-5b50-aa03-7779005ae688" 251 | version = "0.3.27" 252 | 253 | [[Logging]] 254 | uuid = "56ddb016-857b-54e1-b83d-db4d58db5568" 255 | 256 | [[LoopVectorization]] 257 | deps = ["ArrayInterface", "CPUSummary", "ChainRulesCore", "CloseOpenIntervals", "DocStringExtensions", "ForwardDiff", "HostCPUFeatures", "IfElse", "LayoutPointers", "LinearAlgebra", "OffsetArrays", "PolyesterWeave", "PrecompileTools", "SIMDTypes", "SLEEFPirates", "SpecialFunctions", "Static", "StaticArrayInterface", "ThreadingUtilities", "UnPack", "VectorizationBase"] 258 | git-tree-sha1 = "0f5648fbae0d015e3abe5867bca2b362f67a5894" 259 | uuid = "bdcacae8-1622-11e9-2a5c-532679323890" 260 | version = "0.12.166" 261 | 262 | [[MacroTools]] 263 | deps = ["Markdown", "Random"] 264 | git-tree-sha1 = "2fa9ee3e63fd3a4f7a9a4f4744a52f4856de82df" 265 | uuid = "1914dd2f-81c6-5fcd-8719-6d5c9610ff09" 266 | version = "0.5.13" 267 | 268 | [[ManualMemory]] 269 | git-tree-sha1 = "bcaef4fc7a0cfe2cba636d84cda54b5e4e4ca3cd" 270 | uuid = "d125e4d3-2237-4719-b19c-fa641b8a4667" 271 | version = "0.1.8" 272 | 273 | [[Markdown]] 274 | deps = ["Base64"] 275 | uuid = "d6f4376e-aef5-505a-96c1-9c027394607a" 276 | 277 | [[MbedTLS_jll]] 278 | deps = ["Artifacts", "Libdl"] 279 | uuid = "c8ffd9c3-330d-5841-b78e-0817d7145fa1" 280 | 281 | [[Mmap]] 282 | uuid = "a63ad114-7e13-5084-954f-fe012c677804" 283 | 284 | [[MozillaCACerts_jll]] 285 | uuid = "14a3606d-f60d-562e-9121-12d972cd8159" 286 | 287 | [[NaNMath]] 288 | deps = ["OpenLibm_jll"] 289 | git-tree-sha1 = "0877504529a3e5c3343c6f8b4c0381e57e4387e4" 290 | uuid = "77ba4419-2d1f-58cd-9bb1-8ffee604a2e3" 291 | version = "1.0.2" 292 | 293 | [[NetworkOptions]] 294 | uuid = "ca575930-c2e3-43a9-ace4-1e988b2c1908" 295 | 296 | [[Octavian]] 297 | deps = ["CPUSummary", "ForwardDiff", "HyperDualNumbers", "IfElse", "LoopVectorization", "ManualMemory", "PolyesterWeave", "PrecompileTools", "Static", "StaticArrayInterface", "ThreadingUtilities", "VectorizationBase"] 298 | git-tree-sha1 = "e046e337d567faf13634161c44d3d4223ccdadb8" 299 | uuid = "6fd5a793-0b7e-452c-907f-f8bfe9c57db4" 300 | version = "0.3.27" 301 | 302 | [[OffsetArrays]] 303 | deps = ["Adapt"] 304 | git-tree-sha1 = "6a731f2b5c03157418a20c12195eb4b74c8f8621" 305 | uuid = "6fe1bfb0-de20-5000-8ca7-80f57d26f881" 306 | version = "1.13.0" 307 | 308 | [[OpenLibm_jll]] 309 | deps = ["Artifacts", "Libdl"] 310 | uuid = "05823500-19ac-5b8b-9628-191a04bc5112" 311 | 312 | [[OpenSpecFun_jll]] 313 | deps = ["Artifacts", "CompilerSupportLibraries_jll", "JLLWrappers", "Libdl", "Pkg"] 314 | git-tree-sha1 = "13652491f6856acfd2db29360e1bbcd4565d04f1" 315 | uuid = "efe28fd5-8261-553b-a9e1-b2916fc3738e" 316 | version = "0.5.5+0" 317 | 318 | [[Parsers]] 319 | deps = ["Dates", "PrecompileTools", "UUIDs"] 320 | git-tree-sha1 = "8489905bcdbcfac64d1daa51ca07c0d8f0283821" 321 | uuid = "69de0a69-1ddd-5017-9359-2bf0b02dc9f0" 322 | version = "2.8.1" 323 | 324 | [[Pkg]] 325 | deps = ["Artifacts", "Dates", "Downloads", "LibGit2", "Libdl", "Logging", "Markdown", "Printf", "REPL", "Random", "SHA", "Serialization", "TOML", "Tar", "UUIDs", "p7zip_jll"] 326 | uuid = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" 327 | 328 | [[PolyesterWeave]] 329 | deps = ["BitTwiddlingConvenienceFunctions", "CPUSummary", "IfElse", "Static", "ThreadingUtilities"] 330 | git-tree-sha1 = "240d7170f5ffdb285f9427b92333c3463bf65bf6" 331 | uuid = "1d0040c9-8b98-4ee7-8388-3f51789ca0ad" 332 | version = "0.2.1" 333 | 334 | [[PrecompileTools]] 335 | deps = ["Preferences"] 336 | git-tree-sha1 = "5aa36f7049a63a1528fe8f7c3f2113413ffd4e1f" 337 | uuid = "aea7be01-6a6a-4083-8856-8a6e6704d82a" 338 | version = "1.2.1" 339 | 340 | [[Preferences]] 341 | deps = ["TOML"] 342 | git-tree-sha1 = "9306f6085165d270f7e3db02af26a400d580f5c6" 343 | uuid = "21216c6a-2e73-6563-6e65-726566657250" 344 | version = "1.4.3" 345 | 346 | [[Printf]] 347 | deps = ["Unicode"] 348 | uuid = "de0858da-6303-5e67-8744-51eddeeeb8d7" 349 | 350 | [[Profile]] 351 | deps = ["Printf"] 352 | uuid = "9abbd945-dff8-562f-b5e8-e1ebf5ef1b79" 353 | 354 | [[QDLDL_jll]] 355 | deps = ["Artifacts", "JLLWrappers", "Libdl", "Pkg"] 356 | git-tree-sha1 = "54cb0359f294f8b3cbf4ba30208e41d7b8bd116b" 357 | uuid = "9ae34a36-e1b6-54e9-a33d-8bba9913f854" 358 | version = "0.1.5+0" 359 | 360 | [[REPL]] 361 | deps = ["InteractiveUtils", "Markdown", "Sockets", "Unicode"] 362 | uuid = "3fa0cd96-eef1-5676-8a61-b3b8758bbffb" 363 | 364 | [[Random]] 365 | deps = ["Serialization"] 366 | uuid = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" 367 | 368 | [[Ratios]] 369 | deps = ["Requires"] 370 | git-tree-sha1 = "1342a47bf3260ee108163042310d26f2be5ec90b" 371 | uuid = "c84ed2f1-dad5-54f0-aa8e-dbefe2724439" 372 | version = "0.4.5" 373 | 374 | [[RecipesBase]] 375 | deps = ["PrecompileTools"] 376 | git-tree-sha1 = "5c3d09cc4f31f5fc6af001c250bf1278733100ff" 377 | uuid = "3cdcf5f2-1ef4-517c-9805-6587b60abb01" 378 | version = "1.3.4" 379 | 380 | [[Requires]] 381 | deps = ["UUIDs"] 382 | git-tree-sha1 = "838a3a4188e2ded87a4f9f184b4b0d78a1e91cb7" 383 | uuid = "ae029012-a4dd-5104-9daa-d747884805df" 384 | version = "1.3.0" 385 | 386 | [[RobotDynamics]] 387 | deps = ["FiniteDiff", "ForwardDiff", "LinearAlgebra", "Pkg", "RecipesBase", "Rotations", "SparseArrays", "StaticArrays"] 388 | git-tree-sha1 = "45a2cc0d9ee87c1f04486b5a7043262b2c272ae8" 389 | uuid = "38ceca67-d8d3-44e8-9852-78a5596522e1" 390 | version = "0.4.7" 391 | 392 | [[RobotZoo]] 393 | deps = ["FiniteDiff", "ForwardDiff", "LinearAlgebra", "RobotDynamics", "Rotations", "StaticArrays"] 394 | git-tree-sha1 = "e0b5ddc4451c38ab470ac3e5089939824beb89fb" 395 | uuid = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 396 | version = "0.3.1" 397 | 398 | [[Rotations]] 399 | deps = ["LinearAlgebra", "Random", "StaticArrays", "Statistics"] 400 | git-tree-sha1 = "6a23472b6b097d66da87785b61137142ac104f94" 401 | uuid = "6038ab10-8711-5258-84ad-4b1120ba62dc" 402 | version = "1.0.4" 403 | 404 | [[SHA]] 405 | uuid = "ea8e919c-243c-51af-8825-aaa63cd721ce" 406 | 407 | [[SIMDTypes]] 408 | git-tree-sha1 = "330289636fb8107c5f32088d2741e9fd7a061a5c" 409 | uuid = "94e857df-77ce-4151-89e5-788b33177be4" 410 | version = "0.1.0" 411 | 412 | [[SLEEFPirates]] 413 | deps = ["IfElse", "Static", "VectorizationBase"] 414 | git-tree-sha1 = "3aac6d68c5e57449f5b9b865c9ba50ac2970c4cf" 415 | uuid = "476501e8-09a2-5ece-8869-fb82de89a1fa" 416 | version = "0.6.42" 417 | 418 | [[Serialization]] 419 | uuid = "9e88b42a-f829-5b0c-bbe9-9e923198166b" 420 | 421 | [[Setfield]] 422 | deps = ["ConstructionBase", "Future", "MacroTools", "StaticArraysCore"] 423 | git-tree-sha1 = "e2cc6d8c88613c05e1defb55170bf5ff211fbeac" 424 | uuid = "efcf1570-3423-57d1-acb7-fd33fddbac46" 425 | version = "1.1.1" 426 | 427 | [[SharedArrays]] 428 | deps = ["Distributed", "Mmap", "Random", "Serialization"] 429 | uuid = "1a1011a3-84de-559e-8e89-a11a2f7dc383" 430 | 431 | [[Sockets]] 432 | uuid = "6462fe0b-24de-5631-8697-dd941f90decc" 433 | 434 | [[SolverLogging]] 435 | deps = ["Crayons", "Formatting", "Logging", "Printf"] 436 | git-tree-sha1 = "ebe090fc0a0f83685b5bfd7d1eb3fd870bbd1773" 437 | uuid = "c2e08473-88be-4f39-9d3c-afcdb6e3aeb8" 438 | version = "0.2.0" 439 | 440 | [[SparseArrays]] 441 | deps = ["LinearAlgebra", "Random"] 442 | uuid = "2f01184e-e22b-5df5-ae63-d93ebab69eaf" 443 | 444 | [[SpecialFunctions]] 445 | deps = ["ChainRulesCore", "IrrationalConstants", "LogExpFunctions", "OpenLibm_jll", "OpenSpecFun_jll"] 446 | git-tree-sha1 = "e2cfc4012a19088254b3950b85c3c1d8882d864d" 447 | uuid = "276daf66-3868-5448-9aa4-cd146d93841b" 448 | version = "2.3.1" 449 | 450 | [[Static]] 451 | deps = ["IfElse"] 452 | git-tree-sha1 = "b366eb1eb68075745777d80861c6706c33f588ae" 453 | uuid = "aedffcd0-7271-4cad-89d0-dc628f76c6d3" 454 | version = "0.8.9" 455 | 456 | [[StaticArrayInterface]] 457 | deps = ["ArrayInterface", "Compat", "IfElse", "LinearAlgebra", "PrecompileTools", "Requires", "SparseArrays", "Static", "SuiteSparse"] 458 | git-tree-sha1 = "5d66818a39bb04bf328e92bc933ec5b4ee88e436" 459 | uuid = "0d7ed370-da01-4f52-bd93-41d350b8b718" 460 | version = "1.5.0" 461 | 462 | [[StaticArrays]] 463 | deps = ["LinearAlgebra", "PrecompileTools", "Random", "StaticArraysCore", "Statistics"] 464 | git-tree-sha1 = "bf074c045d3d5ffd956fa0a461da38a44685d6b2" 465 | uuid = "90137ffa-7385-5640-81b9-e52037218182" 466 | version = "1.9.3" 467 | 468 | [[StaticArraysCore]] 469 | git-tree-sha1 = "36b3d696ce6366023a0ea192b4cd442268995a0d" 470 | uuid = "1e83bf80-4336-4d27-bf5d-d5a4f845583c" 471 | version = "1.4.2" 472 | 473 | [[Statistics]] 474 | deps = ["LinearAlgebra", "SparseArrays"] 475 | uuid = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" 476 | 477 | [[SuiteSparse]] 478 | deps = ["Libdl", "LinearAlgebra", "Serialization", "SparseArrays"] 479 | uuid = "4607b0f0-06f3-5cda-b6b1-a6196a1729e9" 480 | 481 | [[TOML]] 482 | deps = ["Dates"] 483 | uuid = "fa267f1f-6049-4f14-aa54-33bafae1ed76" 484 | 485 | [[Tar]] 486 | deps = ["ArgTools", "SHA"] 487 | uuid = "a4e569a6-e804-4fa4-b0f3-eef7a1d5b13e" 488 | 489 | [[Test]] 490 | deps = ["InteractiveUtils", "Logging", "Random", "Serialization"] 491 | uuid = "8dfed614-e22c-5e08-85e1-65c5234f0b40" 492 | 493 | [[ThreadingUtilities]] 494 | deps = ["ManualMemory"] 495 | git-tree-sha1 = "eda08f7e9818eb53661b3deb74e3159460dfbc27" 496 | uuid = "8290d209-cae3-49c0-8002-c8c24d57dab5" 497 | version = "0.5.2" 498 | 499 | [[TimerOutputs]] 500 | deps = ["ExprTools", "Printf"] 501 | git-tree-sha1 = "f548a9e9c490030e545f72074a41edfd0e5bcdd7" 502 | uuid = "a759f4b9-e2f1-59dc-863e-4aeb61b1ea8f" 503 | version = "0.5.23" 504 | 505 | [[TrajectoryOptimization]] 506 | deps = ["DocStringExtensions", "FiniteDiff", "ForwardDiff", "LinearAlgebra", "RobotDynamics", "RobotZoo", "Rotations", "SparseArrays", "StaticArrays"] 507 | git-tree-sha1 = "1e449355d1a81f2bb54f7f5aba8ac33b69599130" 508 | uuid = "c79d492b-0548-5874-b488-5a62c1d9d0ca" 509 | version = "0.7.0" 510 | 511 | [[UUIDs]] 512 | deps = ["Random", "SHA"] 513 | uuid = "cf7118a7-6976-5b1a-9a39-7adc72f591a4" 514 | 515 | [[UnPack]] 516 | git-tree-sha1 = "387c1f73762231e86e0c9c5443ce3b4a0a9a0c2b" 517 | uuid = "3a884ed6-31ef-47d7-9d2a-63182c4928ed" 518 | version = "1.0.2" 519 | 520 | [[Unicode]] 521 | uuid = "4ec0a83e-493e-50e2-b9ac-8f72acf5a8f5" 522 | 523 | [[VectorizationBase]] 524 | deps = ["ArrayInterface", "CPUSummary", "HostCPUFeatures", "IfElse", "LayoutPointers", "Libdl", "LinearAlgebra", "SIMDTypes", "Static", "StaticArrayInterface"] 525 | git-tree-sha1 = "7209df901e6ed7489fe9b7aa3e46fb788e15db85" 526 | uuid = "3d5dd08c-fd9d-11e8-17fa-ed2836048c2f" 527 | version = "0.21.65" 528 | 529 | [[WoodburyMatrices]] 530 | deps = ["LinearAlgebra", "SparseArrays"] 531 | git-tree-sha1 = "5f24e158cf4cee437052371455fe361f526da062" 532 | uuid = "efce3f68-66dc-5838-9240-27a6d6f5f9b6" 533 | version = "0.5.6" 534 | 535 | [[Zlib_jll]] 536 | deps = ["Libdl"] 537 | uuid = "83775a58-1f1d-513f-b197-d71354ab007a" 538 | 539 | [[nghttp2_jll]] 540 | deps = ["Artifacts", "Libdl"] 541 | uuid = "8e850ede-7688-5339-a07c-302acd2aaf8d" 542 | 543 | [[p7zip_jll]] 544 | deps = ["Artifacts", "Libdl"] 545 | uuid = "3f19e933-33d8-53b3-aaab-bd5110c3b7a0" 546 | -------------------------------------------------------------------------------- /Lecture 17/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | Altro = "5dcf52e5-e2fb-48e0-b826-96f46d2e3e73" 3 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 4 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 5 | TrajectoryOptimization = "c79d492b-0548-5874-b488-5a62c1d9d0ca" 6 | -------------------------------------------------------------------------------- /Lecture 17/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 18/Lecture 18.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 18/Lecture 18.pdf -------------------------------------------------------------------------------- /Lecture 19/Lecture 19.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 19/Lecture 19.pdf -------------------------------------------------------------------------------- /Lecture 19/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 19/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 19/Project.toml -------------------------------------------------------------------------------- /Lecture 19/lqg.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "f8a3bd70-079d-42e5-925f-0328a8657725", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 11 | ] 12 | }, 13 | { 14 | "cell_type": "code", 15 | "execution_count": null, 16 | "id": "bc71ef22-6de2-4f40-98ce-3d933dfc7106", 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "using LinearAlgebra\n", 21 | "using PyPlot\n", 22 | "using SparseArrays\n", 23 | "using ControlSystems\n", 24 | "using ForwardDiff" 25 | ] 26 | }, 27 | { 28 | "cell_type": "code", 29 | "execution_count": null, 30 | "id": "7e13e806-4bac-4fa0-ab63-679aaa53643b", 31 | "metadata": {}, 32 | "outputs": [], 33 | "source": [ 34 | "# Discrete dynamics\n", 35 | "h = 0.1 # time step\n", 36 | "A = [1 h; 0 1]\n", 37 | "B = [0.5*h*h; h]\n", 38 | "C = [1.0 0]" 39 | ] 40 | }, 41 | { 42 | "cell_type": "code", 43 | "execution_count": null, 44 | "id": "5e3da132-9212-49a2-9d85-8199beae0eab", 45 | "metadata": {}, 46 | "outputs": [], 47 | "source": [ 48 | "# Noise Covariances\n", 49 | "W = B*0.1*B' #Corresponds to white noise force input to dynamics\n", 50 | "V = 0.1 #Noise on position measurements" 51 | ] 52 | }, 53 | { 54 | "cell_type": "code", 55 | "execution_count": null, 56 | "id": "87a50558-c7a0-435e-83ab-c982ceba8bf9", 57 | "metadata": {}, 58 | "outputs": [], 59 | "source": [ 60 | "n = 2 # number of state\n", 61 | "m = 1 # number of controls\n", 62 | "Tfinal = 10.0 # final time\n", 63 | "N = Int(Tfinal/h)+1 # number of time steps\n", 64 | "thist = Array(range(0,h*(N-1), step=h));" 65 | ] 66 | }, 67 | { 68 | "cell_type": "code", 69 | "execution_count": null, 70 | "id": "55ec4049-4cdb-47ca-b37d-9e402f18ad34", 71 | "metadata": {}, 72 | "outputs": [], 73 | "source": [ 74 | "# LQR Cost weights\n", 75 | "Q = Array(1.0*I(n))\n", 76 | "R = Array(0.1*I(m))\n", 77 | "Qn = Q" 78 | ] 79 | }, 80 | { 81 | "cell_type": "code", 82 | "execution_count": null, 83 | "id": "41cbec2b-233b-47ad-9ff1-f16bc7a4207c", 84 | "metadata": {}, 85 | "outputs": [], 86 | "source": [ 87 | "#Cost function\n", 88 | "function J(xhist,uhist)\n", 89 | " cost = 0.5*xhist[:,end]'*Qn*xhist[:,end]\n", 90 | " for k = 1:(size(xhist,2)-1)\n", 91 | " cost = cost + 0.5*xhist[:,k]'*Q*xhist[:,k] + 0.5*(uhist[k]'*R*uhist[k])[1]\n", 92 | " end\n", 93 | " return cost\n", 94 | "end" 95 | ] 96 | }, 97 | { 98 | "cell_type": "code", 99 | "execution_count": null, 100 | "id": "05bafaba-dd76-4f86-8a16-b9cb07f0cb97", 101 | "metadata": {}, 102 | "outputs": [], 103 | "source": [ 104 | "#Infinite-Horizon LQR Gain and cost-to-go\n", 105 | "P = dare(A,B,Q,R)\n", 106 | "K = dlqr(A,B,Q,R)" 107 | ] 108 | }, 109 | { 110 | "cell_type": "code", 111 | "execution_count": null, 112 | "id": "0ce5e70f-c45d-486b-9a99-3cd4c534132f", 113 | "metadata": {}, 114 | "outputs": [], 115 | "source": [ 116 | "# Initial conditions\n", 117 | "x0 = [1.0; 0] #True state\n", 118 | "\n", 119 | "#Filter state\n", 120 | "Σ0 = Array(1.0*I(2))\n", 121 | "x̂0 = [0.0; 0.0]" 122 | ] 123 | }, 124 | { 125 | "cell_type": "code", 126 | "execution_count": null, 127 | "id": "3a91498c-c9f3-44ee-b183-b332fe6c8ffc", 128 | "metadata": {}, 129 | "outputs": [], 130 | "source": [ 131 | "xhist = zeros(n,N)\n", 132 | "xhist[:,1] .= x0;\n", 133 | "\n", 134 | "uhist = zeros(N)\n", 135 | "yhist = zeros(N)\n", 136 | "x̂hist = zeros(n,N)\n", 137 | "Σhist = zeros(n,n,N);" 138 | ] 139 | }, 140 | { 141 | "cell_type": "code", 142 | "execution_count": null, 143 | "id": "dbd85ccc-42e1-4b60-86e8-0797301744a5", 144 | "metadata": {}, 145 | "outputs": [], 146 | "source": [ 147 | "#Initial Time Step\n", 148 | "\n", 149 | "#Generate Measurement\n", 150 | "yhist[1] = (C*xhist[:,1])[1] + sqrt(V)*randn()\n", 151 | "\n", 152 | "z = yhist[1] - (C*x̂0)[1] #Innovation\n", 153 | "S = (C*Σ0*C')[1] + V #Innovation Covariance\n", 154 | "\n", 155 | "L = Σ0*C'*inv(S) #Kalman Gain\n", 156 | "\n", 157 | "x̂hist[:,1] = x̂0 + L*z\n", 158 | "Σhist[:,:,1] .= (I-L*C)*Σ0*(I-L*C)' + L*V*L'\n", 159 | "\n", 160 | "uhist[1] = -(K*x̂hist[:,1])[1] #Control input\n", 161 | "\n", 162 | "xhist[:,2] .= A*xhist[:,1] + B*uhist[1] + sqrt(W)*randn(n) #Simulate with stochastic dynamics" 163 | ] 164 | }, 165 | { 166 | "cell_type": "code", 167 | "execution_count": null, 168 | "id": "6dae90a3-948a-40de-9708-cd5f51c1bd46", 169 | "metadata": {}, 170 | "outputs": [], 171 | "source": [ 172 | "for k = 2:(N-1)\n", 173 | " \n", 174 | " #Generate measurement\n", 175 | " yhist[k] = (C*xhist[:,k])[1] + sqrt(V)*randn()\n", 176 | "\n", 177 | " #KF Update\n", 178 | " x̃ = A*x̂hist[:,k-1] + B*uhist[k-1] #State Prediction\n", 179 | " Σ̃ = A*Σhist[:,:,k-1]*A' + W #Covariance Prediction\n", 180 | "\n", 181 | " z = yhist[k] - (C*x̃)[1] #Innovation\n", 182 | " S = (C*Σ̃*C')[1] + V #Innovation Covariance\n", 183 | "\n", 184 | " L = Σ̃*C'*inv(S) #Kalman Gain\n", 185 | "\n", 186 | " x̂hist[:,k] = x̃ + L*z\n", 187 | " Σhist[:,:,k] = (I-L*C)*Σ̃*(I-L*C)' + L*V*L'\n", 188 | "\n", 189 | " #LQR Controller\n", 190 | " uhist[k] = -(K*x̂hist[:,k])[1]\n", 191 | " \n", 192 | " #Run this on the stochastic dynamics\n", 193 | " xhist[:,k+1] .= A*xhist[:,k] + B*uhist[k] + sqrt(W)*randn(n)\n", 194 | "end" 195 | ] 196 | }, 197 | { 198 | "cell_type": "code", 199 | "execution_count": null, 200 | "id": "b80c5056-0361-4be0-9cc8-f89094aecbb6", 201 | "metadata": {}, 202 | "outputs": [], 203 | "source": [ 204 | "plot(xhist[1,:])\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-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/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-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/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.1))" 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.001)\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 | "cell_type": "code", 225 | "execution_count": null, 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": 4 246 | } 247 | -------------------------------------------------------------------------------- /Lecture 20/Lecture 20.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 20/Lecture 20.pdf -------------------------------------------------------------------------------- /Lecture 21/Lecture 21.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 21/Lecture 21.pdf -------------------------------------------------------------------------------- /Lecture 22/Lecture 22.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 22/Lecture 22.pdf -------------------------------------------------------------------------------- /Lecture 23/Lecture 23.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 23/Lecture 23.pdf -------------------------------------------------------------------------------- /Lecture 23/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 23/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 23/Project.toml -------------------------------------------------------------------------------- /Lecture 3/Lecture 3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/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-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 3/Project.toml -------------------------------------------------------------------------------- /Lecture 3/minimization.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using ForwardDiff\n", 20 | "using PyPlot" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "function f(x)\n", 30 | " return x.^4 + x.^3 - x.^2 - x\n", 31 | "end" 32 | ] 33 | }, 34 | { 35 | "cell_type": "code", 36 | "execution_count": null, 37 | "metadata": {}, 38 | "outputs": [], 39 | "source": [ 40 | "function ∇f(x)\n", 41 | " return 4.0*x.^3 + 3.0*x.^2 - 2.0*x - 1.0\n", 42 | "end" 43 | ] 44 | }, 45 | { 46 | "cell_type": "code", 47 | "execution_count": null, 48 | "metadata": {}, 49 | "outputs": [], 50 | "source": [ 51 | "function ∇2f(x)\n", 52 | " return 12.0*x.^2 + 6.0*x - 2.0\n", 53 | "end" 54 | ] 55 | }, 56 | { 57 | "cell_type": "code", 58 | "execution_count": null, 59 | "metadata": {}, 60 | "outputs": [], 61 | "source": [ 62 | "x = LinRange(-1.75,1.25,1000)" 63 | ] 64 | }, 65 | { 66 | "cell_type": "code", 67 | "execution_count": null, 68 | "metadata": {}, 69 | "outputs": [], 70 | "source": [ 71 | "p = plot(x,f(x))" 72 | ] 73 | }, 74 | { 75 | "cell_type": "code", 76 | "execution_count": null, 77 | "metadata": {}, 78 | "outputs": [], 79 | "source": [ 80 | "function newton_step(x0)\n", 81 | " xn = x0 - ∇2f(x0)\\∇f(x0)\n", 82 | "end" 83 | ] 84 | }, 85 | { 86 | "cell_type": "code", 87 | "execution_count": null, 88 | "metadata": {}, 89 | "outputs": [], 90 | "source": [ 91 | "xguess = 0.0\n", 92 | "plot(x, f(x))\n", 93 | "plot(xguess, f(xguess), \"rx\")" 94 | ] 95 | }, 96 | { 97 | "cell_type": "code", 98 | "execution_count": null, 99 | "metadata": {}, 100 | "outputs": [], 101 | "source": [ 102 | "xnew = newton_step(xguess[end])\n", 103 | "xguess = [xguess xnew]\n", 104 | "plot(x, f(x))\n", 105 | "plot(xguess, f(xguess), \"rx\")" 106 | ] 107 | }, 108 | { 109 | "cell_type": "code", 110 | "execution_count": null, 111 | "metadata": {}, 112 | "outputs": [], 113 | "source": [] 114 | }, 115 | { 116 | "cell_type": "code", 117 | "execution_count": null, 118 | "metadata": {}, 119 | "outputs": [], 120 | "source": [ 121 | "∇2f(0.0)" 122 | ] 123 | }, 124 | { 125 | "cell_type": "code", 126 | "execution_count": null, 127 | "metadata": {}, 128 | "outputs": [], 129 | "source": [ 130 | "function regularized_newton_step(x0)\n", 131 | " β = 1.0\n", 132 | " H = ∇2f(x0)\n", 133 | " while !isposdef(H)\n", 134 | " H = H + β*I\n", 135 | " end\n", 136 | " xn = x0 - H\\∇f(x0)\n", 137 | "end" 138 | ] 139 | }, 140 | { 141 | "cell_type": "code", 142 | "execution_count": null, 143 | "metadata": {}, 144 | "outputs": [], 145 | "source": [ 146 | "xguess = 0.0\n", 147 | "plot(x, f(x))\n", 148 | "plot(xguess, f(xguess), \"rx\")" 149 | ] 150 | }, 151 | { 152 | "cell_type": "code", 153 | "execution_count": null, 154 | "metadata": {}, 155 | "outputs": [], 156 | "source": [ 157 | "xnew = regularized_newton_step(xguess[end])\n", 158 | "xguess = [xguess xnew]\n", 159 | "plot(x, f(x))\n", 160 | "plot(xguess, f(xguess), \"rx\")" 161 | ] 162 | }, 163 | { 164 | "cell_type": "code", 165 | "execution_count": null, 166 | "metadata": {}, 167 | "outputs": [], 168 | "source": [] 169 | }, 170 | { 171 | "cell_type": "code", 172 | "execution_count": null, 173 | "metadata": {}, 174 | "outputs": [], 175 | "source": [] 176 | } 177 | ], 178 | "metadata": { 179 | "kernelspec": { 180 | "display_name": "Julia 1.6.7", 181 | "language": "julia", 182 | "name": "julia-1.6" 183 | }, 184 | "language_info": { 185 | "file_extension": ".jl", 186 | "mimetype": "application/julia", 187 | "name": "julia", 188 | "version": "1.6.7" 189 | } 190 | }, 191 | "nbformat": 4, 192 | "nbformat_minor": 4 193 | } 194 | -------------------------------------------------------------------------------- /Lecture 3/root-finding.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using ForwardDiff\n", 20 | "using PyPlot" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "function pendulum_dynamics(x)\n", 30 | " l = 1.0\n", 31 | " g = 9.81\n", 32 | " \n", 33 | " θ = x[1]\n", 34 | " θ̇ = x[2]\n", 35 | " \n", 36 | " θ̈ = -(g/l)*sin(θ)\n", 37 | " \n", 38 | " return [θ̇; θ̈]\n", 39 | "end" 40 | ] 41 | }, 42 | { 43 | "cell_type": "code", 44 | "execution_count": null, 45 | "metadata": {}, 46 | "outputs": [], 47 | "source": [ 48 | "function backward_euler_step_fixed_point(fun, x0, h)\n", 49 | " xn = x0\n", 50 | " e = [norm(x0 + h.*fun(xn) - xn)]\n", 51 | " while e[end] > 1e-8\n", 52 | " xn = x0 + h.*fun(xn)\n", 53 | " e = [e; norm(x0 + h.*fun(xn) - xn)]\n", 54 | " end\n", 55 | " \n", 56 | " return xn, e\n", 57 | "end" 58 | ] 59 | }, 60 | { 61 | "cell_type": "code", 62 | "execution_count": null, 63 | "metadata": {}, 64 | "outputs": [], 65 | "source": [ 66 | "function backward_euler_step_newton(fun, x0, h)\n", 67 | " xn = x0\n", 68 | " r = x0 + h.*fun(xn) - xn\n", 69 | " e = [norm(r)]\n", 70 | " while e[end] > 1e-8\n", 71 | " ∂r = ForwardDiff.jacobian(x -> x0 + h.*fun(x) - x, xn)\n", 72 | " xn = xn - ∂r\\r\n", 73 | " r = x0 + h.*fun(xn) - xn\n", 74 | " e = [e; norm(r)]\n", 75 | " end\n", 76 | " \n", 77 | " return xn, e\n", 78 | "end" 79 | ] 80 | }, 81 | { 82 | "cell_type": "code", 83 | "execution_count": null, 84 | "metadata": {}, 85 | "outputs": [], 86 | "source": [ 87 | "function backward_euler_fixed_point(fun, x0, Tf, h)\n", 88 | " t = Array(range(0,Tf,step=h))\n", 89 | " \n", 90 | " x_hist = zeros(length(x0),length(t))\n", 91 | " x_hist[:,1] .= x0\n", 92 | " \n", 93 | " for k = 1:(length(t)-1)\n", 94 | " x_hist[:,k+1], e = backward_euler_step_fixed_point(fun, x_hist[:,k], h)\n", 95 | " end\n", 96 | " \n", 97 | " return x_hist, t\n", 98 | "end" 99 | ] 100 | }, 101 | { 102 | "cell_type": "code", 103 | "execution_count": null, 104 | "metadata": {}, 105 | "outputs": [], 106 | "source": [ 107 | "function backward_euler_newton(fun, x0, Tf, h)\n", 108 | " t = Array(range(0,Tf,step=h))\n", 109 | " \n", 110 | " x_hist = zeros(length(x0),length(t))\n", 111 | " x_hist[:,1] .= x0\n", 112 | " \n", 113 | " for k = 1:(length(t)-1)\n", 114 | " x_hist[:,k+1], e = backward_euler_step_newton(fun, x_hist[:,k], h)\n", 115 | " end\n", 116 | " \n", 117 | " return x_hist, t\n", 118 | "end" 119 | ] 120 | }, 121 | { 122 | "cell_type": "code", 123 | "execution_count": null, 124 | "metadata": {}, 125 | "outputs": [], 126 | "source": [ 127 | "x0 = [.1; 0]\n", 128 | "x_hist1, t_hist1 = backward_euler_fixed_point(pendulum_dynamics, x0, 10, 0.01)\n", 129 | "x_hist2, t_hist2 = backward_euler_newton(pendulum_dynamics, x0, 10, 0.01)\n", 130 | "plot(t_hist1, x_hist1[1,:])\n", 131 | "plot(t_hist2, x_hist2[1,:])" 132 | ] 133 | }, 134 | { 135 | "cell_type": "code", 136 | "execution_count": null, 137 | "metadata": {}, 138 | "outputs": [], 139 | "source": [ 140 | "max(abs.(x_hist1-x_hist2)...)" 141 | ] 142 | }, 143 | { 144 | "cell_type": "code", 145 | "execution_count": null, 146 | "metadata": {}, 147 | "outputs": [], 148 | "source": [ 149 | "xn, e1 = backward_euler_step_fixed_point(pendulum_dynamics, x0, 0.1)\n", 150 | "e1" 151 | ] 152 | }, 153 | { 154 | "cell_type": "code", 155 | "execution_count": null, 156 | "metadata": {}, 157 | "outputs": [], 158 | "source": [ 159 | "xn, e2 = backward_euler_step_newton(pendulum_dynamics, x0, 0.1)\n", 160 | "e2" 161 | ] 162 | }, 163 | { 164 | "cell_type": "code", 165 | "execution_count": null, 166 | "metadata": {}, 167 | "outputs": [], 168 | "source": [ 169 | "semilogy(e1)\n", 170 | "semilogy(e2)" 171 | ] 172 | }, 173 | { 174 | "cell_type": "code", 175 | "execution_count": null, 176 | "metadata": {}, 177 | "outputs": [], 178 | "source": [] 179 | }, 180 | { 181 | "cell_type": "code", 182 | "execution_count": null, 183 | "metadata": {}, 184 | "outputs": [], 185 | "source": [] 186 | } 187 | ], 188 | "metadata": { 189 | "kernelspec": { 190 | "display_name": "Julia 1.6.7", 191 | "language": "julia", 192 | "name": "julia-1.6" 193 | }, 194 | "language_info": { 195 | "file_extension": ".jl", 196 | "mimetype": "application/julia", 197 | "name": "julia", 198 | "version": "1.6.7" 199 | } 200 | }, 201 | "nbformat": 4, 202 | "nbformat_minor": 4 203 | } 204 | -------------------------------------------------------------------------------- /Lecture 4/Lecture 4.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/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-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/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-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/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-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/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/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/Lecture 6.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 6/Lecture 6.pdf -------------------------------------------------------------------------------- /Lecture 6/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | -------------------------------------------------------------------------------- /Lecture 6/Project.toml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 6/Project.toml -------------------------------------------------------------------------------- /Lecture 6/control-history.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 6/control-history.pdf -------------------------------------------------------------------------------- /Lecture 6/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 7/Lecture 7.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/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 = 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 = sparse(0.0*I(2))\n", 66 | "R = sparse(0.1*I(1))\n", 67 | "Qn = sparse(10.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 = [10.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 = 10.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-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/Lecture 8/Lecture 8.pdf -------------------------------------------------------------------------------- /Lecture 8/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e" 3 | -------------------------------------------------------------------------------- /Lecture 8/lqr-dp.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 ControlSystems\n", 22 | "using ForwardDiff" 23 | ] 24 | }, 25 | { 26 | "cell_type": "code", 27 | "execution_count": null, 28 | "metadata": {}, 29 | "outputs": [], 30 | "source": [ 31 | "# Discrete dynamics\n", 32 | "h = 0.1 # time step\n", 33 | "A = [1 h; 0 1]\n", 34 | "B = [0.5*h*h; h]" 35 | ] 36 | }, 37 | { 38 | "cell_type": "code", 39 | "execution_count": null, 40 | "metadata": {}, 41 | "outputs": [], 42 | "source": [ 43 | "#Controllability\n", 44 | "rank([B A*B])" 45 | ] 46 | }, 47 | { 48 | "cell_type": "code", 49 | "execution_count": null, 50 | "metadata": {}, 51 | "outputs": [], 52 | "source": [ 53 | "n = 2 # number of state\n", 54 | "m = 1 # number of controls\n", 55 | "Tfinal = 10.0 # final time\n", 56 | "N = Int(Tfinal/h)+1 # number of time steps\n", 57 | "thist = Array(range(0,h*(N-1), step=h));" 58 | ] 59 | }, 60 | { 61 | "cell_type": "code", 62 | "execution_count": null, 63 | "metadata": {}, 64 | "outputs": [], 65 | "source": [ 66 | "# Initial conditions\n", 67 | "x0 = [1.0; 0]" 68 | ] 69 | }, 70 | { 71 | "cell_type": "code", 72 | "execution_count": null, 73 | "metadata": {}, 74 | "outputs": [], 75 | "source": [ 76 | "# Cost weights\n", 77 | "Q = Array(1.0*I(n))\n", 78 | "R = Array(0.1*I(m))\n", 79 | "Qn = Array(1.0*I(n))" 80 | ] 81 | }, 82 | { 83 | "cell_type": "code", 84 | "execution_count": null, 85 | "metadata": {}, 86 | "outputs": [], 87 | "source": [ 88 | "#Cost function\n", 89 | "function J(xhist,uhist)\n", 90 | " cost = 0.5*xhist[:,end]'*Qn*xhist[:,end]\n", 91 | " for k = 1:(size(xhist,2)-1)\n", 92 | " cost = cost + 0.5*xhist[:,k]'*Q*xhist[:,k] + 0.5*(uhist[k]'*R*uhist[k])[1]\n", 93 | " end\n", 94 | " return cost\n", 95 | "end" 96 | ] 97 | }, 98 | { 99 | "cell_type": "code", 100 | "execution_count": null, 101 | "metadata": {}, 102 | "outputs": [], 103 | "source": [ 104 | "#Cost-to-go Function\n", 105 | "function Vinf(x)\n", 106 | " return 0.5*x'*Pinf*x\n", 107 | "end\n", 108 | "\n", 109 | "function V(k,x)\n", 110 | " return 0.5*x'*P[:,:,k]*x\n", 111 | "end" 112 | ] 113 | }, 114 | { 115 | "cell_type": "code", 116 | "execution_count": null, 117 | "metadata": {}, 118 | "outputs": [], 119 | "source": [ 120 | "#QP Solution for xhist, uhist\n", 121 | "# Cost\n", 122 | "H = blockdiag(sparse(R), kron(I(N-2), blockdiag(sparse(Q),sparse(R))), sparse(Qn));" 123 | ] 124 | }, 125 | { 126 | "cell_type": "code", 127 | "execution_count": null, 128 | "metadata": {}, 129 | "outputs": [], 130 | "source": [ 131 | "# Constraints\n", 132 | "C = kron(I(N-1), [B -I(2)])\n", 133 | "for k = 1:N-2\n", 134 | " C[(k*n).+(1:n), (k*(n+m)-n).+(1:n)] .= A\n", 135 | "end\n", 136 | "d = [-A*x0; zeros(size(C,1)-n)];" 137 | ] 138 | }, 139 | { 140 | "cell_type": "code", 141 | "execution_count": null, 142 | "metadata": {}, 143 | "outputs": [], 144 | "source": [ 145 | "# Solve the linear system\n", 146 | "y = [H C'; C zeros(size(C,1),size(C,1))]\\[zeros(size(H,1)); d]\n", 147 | "\n", 148 | "# Get multipliers\n", 149 | "λhist_qp = reshape(y[(size(H,1)+1):end],n,N-1)\n", 150 | "\n", 151 | "# Get state history\n", 152 | "z = y[1:size(H,1)] # states and controls [u0,x1,u1,...,xN]\n", 153 | "Z = reshape(z,n+m,N-1)\n", 154 | "xhist_qp = Z[m+1:end,:]\n", 155 | "xhist_qp = [x0 xhist_qp]\n", 156 | "\n", 157 | "# Get control history\n", 158 | "uhist_qp = Z[1,:];" 159 | ] 160 | }, 161 | { 162 | "cell_type": "code", 163 | "execution_count": null, 164 | "metadata": {}, 165 | "outputs": [], 166 | "source": [ 167 | "#Dynamic Programming Solution for P and K\n", 168 | "P = zeros(n,n,N)\n", 169 | "K = zeros(m,n,N-1)\n", 170 | "\n", 171 | "P[:,:,N] .= Qn\n", 172 | "\n", 173 | "#Backward Riccati recursion\n", 174 | "for k = (N-1):-1:1\n", 175 | " K[:,:,k] .= (R .+ B'*P[:,:,k+1]*B)\\(B'*P[:,:,k+1]*A)\n", 176 | " P[:,:,k] .= Q + K[:,:,k]'*R*K[:,:,k] + (A-B*K[:,:,k])'*P[:,:,k+1]*(A-B*K[:,:,k])\n", 177 | "end\n", 178 | "\n", 179 | "#Forward rollout starting at x0\n", 180 | "xhist = zeros(n,N)\n", 181 | "xhist[:,1] = x0\n", 182 | "uhist = zeros(m,N-1)\n", 183 | "for k = 1:(N-1)\n", 184 | " uhist[:,k] .= -K[:,:,k]*xhist[:,k]\n", 185 | " xhist[:,k+1] .= A*xhist[:,k] + B*uhist[k]\n", 186 | "end" 187 | ] 188 | }, 189 | { 190 | "cell_type": "code", 191 | "execution_count": null, 192 | "metadata": {}, 193 | "outputs": [], 194 | "source": [ 195 | "# Plot x1 vs. x2, u vs. t, x vs. t, etc.\n", 196 | "times = range(0,h*(N-1), step=h)\n", 197 | "plot(times,xhist[1,:], label=\"DP position\")\n", 198 | "plot(times,xhist[2,:], label=\"DP velocity\")\n", 199 | "plot(times,xhist_qp[1,:], label=\"QP position\")\n", 200 | "plot(times,xhist_qp[2,:], label=\"QP velocity\")\n", 201 | "xlabel(\"time\")\n", 202 | "legend()" 203 | ] 204 | }, 205 | { 206 | "cell_type": "code", 207 | "execution_count": null, 208 | "metadata": {}, 209 | "outputs": [], 210 | "source": [ 211 | "plot(times[1:end-1], uhist[1,:], label=\"DP control\")\n", 212 | "plot(times[1:end-1], uhist_qp, label=\"QP control\")\n", 213 | "xlabel(\"Time\")\n", 214 | "legend()" 215 | ] 216 | }, 217 | { 218 | "cell_type": "code", 219 | "execution_count": null, 220 | "metadata": {}, 221 | "outputs": [], 222 | "source": [ 223 | "plot(P[1,1,:])\n", 224 | "plot(P[1,2,:])\n", 225 | "plot(P[2,2,:])" 226 | ] 227 | }, 228 | { 229 | "cell_type": "code", 230 | "execution_count": null, 231 | "metadata": {}, 232 | "outputs": [], 233 | "source": [ 234 | "J(xhist,uhist)" 235 | ] 236 | }, 237 | { 238 | "cell_type": "code", 239 | "execution_count": null, 240 | "metadata": {}, 241 | "outputs": [], 242 | "source": [ 243 | "J(xhist_qp,uhist_qp)" 244 | ] 245 | }, 246 | { 247 | "cell_type": "code", 248 | "execution_count": null, 249 | "metadata": {}, 250 | "outputs": [], 251 | "source": [ 252 | "#Compute infinite-horizon K matrix using ControlSystems.jl\n", 253 | "Kinf = dlqr(A,B,Q,R[1])\n", 254 | "#Compare to ours\n", 255 | "K[:,:,1]-Kinf" 256 | ] 257 | }, 258 | { 259 | "cell_type": "code", 260 | "execution_count": null, 261 | "metadata": {}, 262 | "outputs": [], 263 | "source": [ 264 | "#Compute infinite-horizon P matrix\n", 265 | "Pinf = dare(A,B,Q,R)\n", 266 | "#Compare to ours\n", 267 | "P[:,:,1] - Pinf" 268 | ] 269 | }, 270 | { 271 | "cell_type": "code", 272 | "execution_count": null, 273 | "metadata": {}, 274 | "outputs": [], 275 | "source": [ 276 | "#Forward rollout starting at xk\n", 277 | "function rollout(k,x)\n", 278 | " xsub = zeros(n,N-k+1)\n", 279 | " xsub[:,1] .= x\n", 280 | " usub = zeros(m,N-k)\n", 281 | " for j = k:(N-1)\n", 282 | " usub[:,j-k+1] .= -K[:,:,j]*xsub[:,j-k+1]\n", 283 | " xsub[:,j-k+2] .= A*xsub[:,j-k+1] + B*usub[j-k+1]\n", 284 | " end\n", 285 | " return xsub,usub\n", 286 | "end" 287 | ] 288 | }, 289 | { 290 | "cell_type": "code", 291 | "execution_count": null, 292 | "metadata": {}, 293 | "outputs": [], 294 | "source": [ 295 | "#Generate a sub-trajectory starting at xk\n", 296 | "k = 50\n", 297 | "xsub, usub = rollout(k,xhist[:,k])" 298 | ] 299 | }, 300 | { 301 | "cell_type": "code", 302 | "execution_count": null, 303 | "metadata": {}, 304 | "outputs": [], 305 | "source": [ 306 | "#Optimal state sub-trajectories are optimal\n", 307 | "xsub - xhist[:,k:end]" 308 | ] 309 | }, 310 | { 311 | "cell_type": "code", 312 | "execution_count": null, 313 | "metadata": {}, 314 | "outputs": [], 315 | "source": [ 316 | "#Optimal control sub-trajectories are optimal\n", 317 | "usub - uhist[:,k:end]" 318 | ] 319 | }, 320 | { 321 | "cell_type": "code", 322 | "execution_count": null, 323 | "metadata": {}, 324 | "outputs": [], 325 | "source": [ 326 | "#Compare multipliers from QP to Cost-to-go gradient from DP\n", 327 | "λhist_qp[:,k-1]" 328 | ] 329 | }, 330 | { 331 | "cell_type": "code", 332 | "execution_count": null, 333 | "metadata": {}, 334 | "outputs": [], 335 | "source": [ 336 | "ForwardDiff.gradient(x->V(k,x),xhist[:,k])" 337 | ] 338 | }, 339 | { 340 | "cell_type": "code", 341 | "execution_count": null, 342 | "metadata": {}, 343 | "outputs": [], 344 | "source": [ 345 | "#Also compare to infinite horizon\n", 346 | "ForwardDiff.gradient(x->Vinf(x),xhist[:,k])" 347 | ] 348 | }, 349 | { 350 | "cell_type": "code", 351 | "execution_count": null, 352 | "metadata": {}, 353 | "outputs": [], 354 | "source": [ 355 | "#Let's try finite diffing the cost w.r.t. the state\n", 356 | "x1p, u1p = rollout(k,xhist[:,k]+[1e-6; 0])\n", 357 | "x2p, u2p = rollout(k,xhist[:,k]+[0; 1e-6])\n", 358 | "λfd = [J(x1p,u1p) - J(xhist[:,k:end],uhist[:,k:end]), J(x2p,u2p) - J(xhist[:,k:end],uhist[:,k:end])]./1e-6" 359 | ] 360 | }, 361 | { 362 | "cell_type": "code", 363 | "execution_count": null, 364 | "metadata": {}, 365 | "outputs": [], 366 | "source": [] 367 | } 368 | ], 369 | "metadata": { 370 | "kernelspec": { 371 | "display_name": "Julia 1.6.7", 372 | "language": "julia", 373 | "name": "julia-1.6" 374 | }, 375 | "language_info": { 376 | "file_extension": ".jl", 377 | "mimetype": "application/julia", 378 | "name": "julia", 379 | "version": "1.6.7" 380 | } 381 | }, 382 | "nbformat": 4, 383 | "nbformat_minor": 4 384 | } 385 | -------------------------------------------------------------------------------- /Lecture 9/Lecture 9.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks-2024/72a049922d84ca573a8b551ac8a6b842fc81cb70/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 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 7 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 8 | Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" 9 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 10 | -------------------------------------------------------------------------------- /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 2024. 4 | 5 |

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

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