├── .gitignore ├── Lecture 1 ├── Intro.pdf └── Lecture 1.pdf ├── Lecture 10 ├── Lecture 10.pdf ├── Manifest.toml ├── Project.toml ├── mpc.ipynb └── quadrotor_scaled.obj ├── Lecture 11 ├── Lecture 11.pdf ├── Manifest.toml ├── Project.toml └── acrobot-ilqr.ipynb ├── Lecture 12 ├── Lecture 12.pdf ├── Manifest.toml ├── Project.toml ├── acrobot.ipynb ├── cartpole.ipynb └── guess.jld2 ├── Lecture 13 ├── Lecture 13.pdf ├── Manifest.toml ├── Project.toml └── dircol.ipynb ├── Lecture 14 ├── Lecture 14.pdf ├── Manifest.toml ├── Project.toml └── rbsim.ipynb ├── Lecture 15 ├── Lecture 15.pdf ├── Manifest.toml └── wahba.ipynb ├── Lecture 16 ├── Lecture 16.pdf ├── Manifest.toml ├── Project.toml └── quadrotor.ipynb ├── Lecture 17 ├── Lecture 17.pdf ├── Manifest.toml ├── Project.toml ├── hopper.ipynb └── hybrid-ball.ipynb ├── Lecture 18 ├── Lecture 18.pdf ├── Manifest.toml ├── Project.toml └── cartpole-ilc.ipynb ├── Lecture 19 └── Lecture 19.pdf ├── Lecture 2 ├── Lecture 2.pdf ├── Manifest.toml ├── Project.toml └── integrators.ipynb ├── Lecture 20 └── Lecture 20.pdf ├── Lecture 21 ├── Lecture 21.pdf ├── Manifest.toml ├── Project.toml └── lqg.ipynb ├── Lecture 22 └── Lecture 22.pdf ├── Lecture 23 └── Lecture 23.pdf ├── Lecture 24 ├── Lecture 24.pdf ├── Manifest.toml ├── Project.toml ├── oscillator.ipynb ├── quadrotor-clone.ipynb └── quadrotor_scaled.obj ├── 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 └── interior-point.ipynb ├── Lecture 6 ├── Lecture 6.pdf ├── Manifest.toml ├── Project.toml ├── merit-functions.ipynb └── regularization.ipynb ├── Lecture 7 ├── Lecture 7.pdf ├── Manifest.toml ├── Project.toml └── lqr-shooting.ipynb ├── Lecture 8 ├── Lecture 8.pdf ├── Manifest.toml ├── Project.toml ├── lqr-qp.ipynb └── lqr-riccati.ipynb ├── Lecture 9 ├── Lecture 9.pdf ├── Manifest.toml ├── Project.toml └── lqr-dp.ipynb └── 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/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 1/Intro.pdf -------------------------------------------------------------------------------- /Lecture 1/Lecture 1.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 1/Lecture 1.pdf -------------------------------------------------------------------------------- /Lecture 10/Lecture 10.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 10/Lecture 10.pdf -------------------------------------------------------------------------------- /Lecture 10/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298" 3 | MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11" 4 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 5 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 6 | Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" 7 | -------------------------------------------------------------------------------- /Lecture 10/mpc.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using PyPlot\n", 20 | "using SparseArrays\n", 21 | "using ForwardDiff\n", 22 | "using ControlSystems\n", 23 | "using OSQP" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": null, 29 | "metadata": {}, 30 | "outputs": [], 31 | "source": [ 32 | "#Model parameters\n", 33 | "g = 9.81 #m/s^2\n", 34 | "m = 1.0 #kg \n", 35 | "ℓ = 0.3 #meters\n", 36 | "J = 0.2*m*ℓ*ℓ\n", 37 | "\n", 38 | "#Thrust limits\n", 39 | "umin = [0.2*m*g; 0.2*m*g]\n", 40 | "umax = [0.6*m*g; 0.6*m*g]\n", 41 | "\n", 42 | "h = 0.05 #time step (20 Hz)" 43 | ] 44 | }, 45 | { 46 | "cell_type": "code", 47 | "execution_count": null, 48 | "metadata": {}, 49 | "outputs": [], 50 | "source": [ 51 | "#Planar Quadrotor Dynamics\n", 52 | "function quad_dynamics(x,u)\n", 53 | " θ = x[3]\n", 54 | " \n", 55 | " ẍ = (1/m)*(u[1] + u[2])*sin(θ)\n", 56 | " ÿ = (1/m)*(u[1] + u[2])*cos(θ) - g\n", 57 | " θ̈ = (1/J)*(ℓ/2)*(u[2] - u[1])\n", 58 | " \n", 59 | " return [x[4:6]; ẍ; ÿ; θ̈]\n", 60 | "end" 61 | ] 62 | }, 63 | { 64 | "cell_type": "code", 65 | "execution_count": null, 66 | "metadata": {}, 67 | "outputs": [], 68 | "source": [ 69 | "function quad_dynamics_rk4(x,u)\n", 70 | " #RK4 integration with zero-order hold on u\n", 71 | " f1 = quad_dynamics(x, u)\n", 72 | " f2 = quad_dynamics(x + 0.5*h*f1, u)\n", 73 | " f3 = quad_dynamics(x + 0.5*h*f2, u)\n", 74 | " f4 = quad_dynamics(x + h*f3, u)\n", 75 | " return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 76 | "end" 77 | ] 78 | }, 79 | { 80 | "cell_type": "code", 81 | "execution_count": null, 82 | "metadata": {}, 83 | "outputs": [], 84 | "source": [ 85 | "#Linearized dynamics for hovering\n", 86 | "x_hover = zeros(6)\n", 87 | "u_hover = [0.5*m*g; 0.5*m*g]\n", 88 | "A = ForwardDiff.jacobian(x->quad_dynamics_rk4(x,u_hover),x_hover);\n", 89 | "B = ForwardDiff.jacobian(u->quad_dynamics_rk4(x_hover,u),u_hover);\n", 90 | "quad_dynamics_rk4(x_hover, u_hover)" 91 | ] 92 | }, 93 | { 94 | "cell_type": "code", 95 | "execution_count": null, 96 | "metadata": {}, 97 | "outputs": [], 98 | "source": [ 99 | "Nx = 6 # number of state\n", 100 | "Nu = 2 # number of controls\n", 101 | "Tfinal = 10.0 # final time\n", 102 | "Nt = Int(Tfinal/h)+1 # number of time steps\n", 103 | "thist = Array(range(0,h*(Nt-1), step=h));" 104 | ] 105 | }, 106 | { 107 | "cell_type": "code", 108 | "execution_count": null, 109 | "metadata": {}, 110 | "outputs": [], 111 | "source": [ 112 | "# Cost weights\n", 113 | "Q = Array(1.0*I(Nx));\n", 114 | "R = Array(.01*I(Nu));\n", 115 | "Qn = Array(1.0*I(Nx));" 116 | ] 117 | }, 118 | { 119 | "cell_type": "code", 120 | "execution_count": null, 121 | "metadata": {}, 122 | "outputs": [], 123 | "source": [ 124 | "#Cost function\n", 125 | "function cost(xhist,uhist)\n", 126 | " cost = 0.5*xhist[:,end]'*Qn*xhist[:,end]\n", 127 | " for k = 1:(size(xhist,2)-1)\n", 128 | " cost = cost + 0.5*xhist[:,k]'*Q*xhist[:,k] + 0.5*(uhist[k]'*R*uhist[k])[1]\n", 129 | " end\n", 130 | " return cost\n", 131 | "end" 132 | ] 133 | }, 134 | { 135 | "cell_type": "code", 136 | "execution_count": null, 137 | "metadata": {}, 138 | "outputs": [], 139 | "source": [ 140 | "#LQR Hover Controller\n", 141 | "P = dare(A,B,Q,R)\n", 142 | "K = dlqr(A,B,Q,R)\n", 143 | "\n", 144 | "function lqr_controller(t,x,K,xref)\n", 145 | " \n", 146 | " return u_hover - K*(x-xref)\n", 147 | "end" 148 | ] 149 | }, 150 | { 151 | "cell_type": "code", 152 | "execution_count": null, 153 | "metadata": {}, 154 | "outputs": [], 155 | "source": [ 156 | "#Build QP matrices for OSQP\n", 157 | "Nh = 20 #one second horizon at 20Hz\n", 158 | "Nx = 6\n", 159 | "Nu = 2\n", 160 | "U = kron(Diagonal(I,Nh), [I zeros(Nu,Nx)]) #Matrix that picks out all u\n", 161 | "Θ = kron(Diagonal(I,Nh), [0 0 0 0 1 0 0 0]) #Matrix that picks out all x3 (θ)\n", 162 | "H = sparse([kron(Diagonal(I,Nh-1),[R zeros(Nu,Nx); zeros(Nx,Nu) Q]) zeros((Nx+Nu)*(Nh-1), Nx+Nu); zeros(Nx+Nu,(Nx+Nu)*(Nh-1)) [R zeros(Nu,Nx); zeros(Nx,Nu) P]])\n", 163 | "b = zeros(Nh*(Nx+Nu))\n", 164 | "C = sparse([[B -I zeros(Nx,(Nh-1)*(Nu+Nx))]; zeros(Nx*(Nh-1),Nu) [kron(Diagonal(I,Nh-1), [A B]) zeros((Nh-1)*Nx,Nx)] + [zeros((Nh-1)*Nx,Nx) kron(Diagonal(I,Nh-1),[zeros(Nx,Nu) Diagonal(-I,Nx)])]])\n", 165 | "\n", 166 | "#Dynamics + Thrust limit constraints\n", 167 | "D = [C; U]\n", 168 | "lb = [zeros(Nx*Nh); kron(ones(Nh),umin-u_hover)]\n", 169 | "ub = [zeros(Nx*Nh); kron(ones(Nh),umax-u_hover)]\n", 170 | "\n", 171 | "#Dynamics + thrust limit + bound constraint on θ to keep the system within small-angle approximation\n", 172 | "#D = [C; U; Θ]\n", 173 | "#lb = [zeros(Nx*Nh); kron(ones(Nh),umin-u_hover); -0.2*ones(Nh)]\n", 174 | "#ub = [zeros(Nx*Nh); kron(ones(Nh),umax-u_hover); 0.2*ones(Nh)]\n", 175 | "\n", 176 | "prob = OSQP.Model()\n", 177 | "OSQP.setup!(prob; P=H, q=b, A=D, l=lb, u=ub, verbose=false, eps_abs=1e-8, eps_rel=1e-8, polish=1);" 178 | ] 179 | }, 180 | { 181 | "cell_type": "code", 182 | "execution_count": null, 183 | "metadata": {}, 184 | "outputs": [], 185 | "source": [ 186 | "#MPC Controller\n", 187 | "function mpc_controller(t,x,xref)\n", 188 | " \n", 189 | " #Update QP problem\n", 190 | " lb[1:6] .= -A*x\n", 191 | " ub[1:6] .= -A*x\n", 192 | " \n", 193 | " for j = 1:(Nh-1)\n", 194 | " b[(Nu+(j-1)*(Nx+Nu)).+(1:Nx)] .= -Q*xref\n", 195 | " end\n", 196 | " b[(Nu+(Nh-1)*(Nx+Nu)).+(1:Nx)] .= -P*xref\n", 197 | " \n", 198 | " OSQP.update!(prob, q=b, l=lb, u=ub)\n", 199 | "\n", 200 | " #Solve QP\n", 201 | " results = OSQP.solve!(prob)\n", 202 | " Δu = results.x[1:Nu]\n", 203 | "\n", 204 | " return u_hover + Δu\n", 205 | "end" 206 | ] 207 | }, 208 | { 209 | "cell_type": "code", 210 | "execution_count": null, 211 | "metadata": {}, 212 | "outputs": [], 213 | "source": [ 214 | "function closed_loop(x0,controller,N)\n", 215 | " xhist = zeros(length(x0),N)\n", 216 | " u0 = controller(1,x0)\n", 217 | " uhist = zeros(length(u0),N-1)\n", 218 | " uhist[:,1] .= u0\n", 219 | " xhist[:,1] .= x0\n", 220 | " for k = 1:(N-1)\n", 221 | " uk = controller(k,xhist[:,k])\n", 222 | " uhist[:,k] = max.(min.(umax, uk), umin) #enforce control limits\n", 223 | " xhist[:,k+1] .= quad_dynamics_rk4(xhist[:,k],uhist[:,k])\n", 224 | " end\n", 225 | " return xhist, uhist\n", 226 | "end" 227 | ] 228 | }, 229 | { 230 | "cell_type": "code", 231 | "execution_count": null, 232 | "metadata": {}, 233 | "outputs": [], 234 | "source": [ 235 | "x_ref = [0.0; 1.0; 0; 0; 0; 0]\n", 236 | "x0 = [1.0; 2.0; 0.0; 0; 0; 0]\n", 237 | "xhist1, uhist1 = closed_loop(x0, (t,x)->lqr_controller(t,x,K,x_ref), Nt);\n", 238 | "xhist2, uhist2 = closed_loop(x0, (t,x)->mpc_controller(t,x,x_ref), Nt);" 239 | ] 240 | }, 241 | { 242 | "cell_type": "code", 243 | "execution_count": null, 244 | "metadata": {}, 245 | "outputs": [], 246 | "source": [ 247 | "plot(thist,xhist1[1,:], label=\"x LQR\")\n", 248 | "plot(thist,xhist2[1,:], label=\"x MPC\")\n", 249 | "xlabel(\"time\")\n", 250 | "legend()" 251 | ] 252 | }, 253 | { 254 | "cell_type": "code", 255 | "execution_count": null, 256 | "metadata": {}, 257 | "outputs": [], 258 | "source": [ 259 | "plot(thist,xhist1[2,:], label=\"y LQR\")\n", 260 | "plot(thist,xhist2[2,:], label=\"y MPC\")\n", 261 | "xlabel(\"time\")\n", 262 | "legend()" 263 | ] 264 | }, 265 | { 266 | "cell_type": "code", 267 | "execution_count": null, 268 | "metadata": {}, 269 | "outputs": [], 270 | "source": [ 271 | "#plot(thist,xhist1[3,:], label=\"θ LQR\")\n", 272 | "plot(thist,xhist2[3,:], label=\"θ MPC\")\n", 273 | "xlabel(\"time\")\n", 274 | "legend()" 275 | ] 276 | }, 277 | { 278 | "cell_type": "code", 279 | "execution_count": null, 280 | "metadata": {}, 281 | "outputs": [], 282 | "source": [ 283 | "plot(thist[1:end-1], uhist1[1,:], label=\"u1 LQR\")\n", 284 | "plot(thist[1:end-1], uhist2[1,:], label=\"u1 MPC\")\n", 285 | "xlabel(\"Time\")\n", 286 | "legend()" 287 | ] 288 | }, 289 | { 290 | "cell_type": "code", 291 | "execution_count": null, 292 | "metadata": {}, 293 | "outputs": [], 294 | "source": [ 295 | "plot(thist[1:end-1], uhist1[2,:], label=\"u2 LQR\")\n", 296 | "plot(thist[1:end-1], uhist2[2,:], label=\"u2 MPC\")\n", 297 | "xlabel(\"Time\")\n", 298 | "legend()" 299 | ] 300 | }, 301 | { 302 | "cell_type": "code", 303 | "execution_count": null, 304 | "metadata": {}, 305 | "outputs": [], 306 | "source": [ 307 | "#Set up visualization\n", 308 | "using MeshCat\n", 309 | "using RobotZoo: Quadrotor, PlanarQuadrotor\n", 310 | "using CoordinateTransformations, Rotations, Colors, StaticArrays, RobotDynamics\n", 311 | "\n", 312 | "function set_mesh!(vis, model::L;\n", 313 | " scaling=1.0, color=colorant\"black\"\n", 314 | " ) where {L <: Union{Quadrotor, PlanarQuadrotor}} \n", 315 | " # urdf_folder = joinpath(@__DIR__, \"..\", \"data\", \"meshes\")\n", 316 | " urdf_folder = @__DIR__\n", 317 | " # if scaling != 1.0\n", 318 | " # quad_scaling = 0.085 * scaling\n", 319 | " obj = joinpath(urdf_folder, \"quadrotor_scaled.obj\")\n", 320 | " if scaling != 1.0\n", 321 | " error(\"Scaling not implemented after switching to MeshCat 0.12\")\n", 322 | " end\n", 323 | " robot_obj = MeshFileGeometry(obj)\n", 324 | " mat = MeshPhongMaterial(color=color)\n", 325 | " setobject!(vis[\"robot\"][\"geom\"], robot_obj, mat)\n", 326 | " if hasfield(L, :ned)\n", 327 | " model.ned && settransform!(vis[\"robot\"][\"geom\"], LinearMap(RotX(pi)))\n", 328 | " end\n", 329 | "end\n", 330 | "\n", 331 | "function visualize!(vis, model::PlanarQuadrotor, x::StaticVector)\n", 332 | " py,pz = x[1], x[2]\n", 333 | " θ = x[3]\n", 334 | " settransform!(vis[\"robot\"], compose(Translation(0,py,pz), LinearMap(RotX(-θ))))\n", 335 | "end\n", 336 | "\n", 337 | "function visualize!(vis, model, tf::Real, X)\n", 338 | " fps = Int(round((length(X)-1)/tf))\n", 339 | " anim = MeshCat.Animation(vis; 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 | "scrolled": true 355 | }, 356 | "outputs": [], 357 | "source": [ 358 | "vis = Visualizer()\n", 359 | "model = PlanarQuadrotor()\n", 360 | "set_mesh!(vis, model)\n", 361 | "render(vis)" 362 | ] 363 | }, 364 | { 365 | "cell_type": "code", 366 | "execution_count": null, 367 | "metadata": {}, 368 | "outputs": [], 369 | "source": [ 370 | "X1 = [SVector{6}(x) for x in eachcol(xhist1)];\n", 371 | "X2 = [SVector{6}(x) for x in eachcol(xhist2)];" 372 | ] 373 | }, 374 | { 375 | "cell_type": "code", 376 | "execution_count": null, 377 | "metadata": {}, 378 | "outputs": [], 379 | "source": [ 380 | "visualize!(vis, model, thist[end], X1)" 381 | ] 382 | }, 383 | { 384 | "cell_type": "code", 385 | "execution_count": null, 386 | "metadata": {}, 387 | "outputs": [], 388 | "source": [ 389 | "visualize!(vis, model, thist[end], X2)" 390 | ] 391 | }, 392 | { 393 | "cell_type": "code", 394 | "execution_count": null, 395 | "metadata": {}, 396 | "outputs": [], 397 | "source": [] 398 | } 399 | ], 400 | "metadata": { 401 | "kernelspec": { 402 | "display_name": "Julia 1.10.8", 403 | "language": "julia", 404 | "name": "julia-1.10" 405 | }, 406 | "language_info": { 407 | "file_extension": ".jl", 408 | "mimetype": "application/julia", 409 | "name": "julia", 410 | "version": "1.10.8" 411 | } 412 | }, 413 | "nbformat": 4, 414 | "nbformat_minor": 4 415 | } 416 | -------------------------------------------------------------------------------- /Lecture 11/Lecture 11.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 11/Lecture 11.pdf -------------------------------------------------------------------------------- /Lecture 11/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | julia_version = "1.10.8" 4 | manifest_format = "2.0" 5 | project_hash = "da39a3ee5e6b4b0d3255bfef95601890afd80709" 6 | 7 | [deps] 8 | -------------------------------------------------------------------------------- /Lecture 11/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | -------------------------------------------------------------------------------- /Lecture 11/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])\n", 234 | " B = dfdu(xtraj[:,k], 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(vis; fps=floor(Int, 1.0/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.10.8", 372 | "language": "julia", 373 | "name": "julia-1.10" 374 | }, 375 | "language_info": { 376 | "file_extension": ".jl", 377 | "mimetype": "application/julia", 378 | "name": "julia", 379 | "version": "1.10.8" 380 | } 381 | }, 382 | "nbformat": 4, 383 | "nbformat_minor": 4 384 | } 385 | -------------------------------------------------------------------------------- /Lecture 12/Lecture 12.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 12/Lecture 12.pdf -------------------------------------------------------------------------------- /Lecture 12/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | MatrixCalculus = "fafa779c-62f8-11e9-0c41-4d0261472a5f" 3 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 4 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 5 | -------------------------------------------------------------------------------- /Lecture 12/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.01;\n", 114 | "Qn = Array(100.0*I(Nx));" 115 | ] 116 | }, 117 | { 118 | "cell_type": "code", 119 | "execution_count": null, 120 | "metadata": {}, 121 | "outputs": [], 122 | "source": [ 123 | "function stage_cost(x,u)\n", 124 | " return 0.5*((x-xgoal)'*Q*(x-xgoal)) + 0.5*R*u*u\n", 125 | "end" 126 | ] 127 | }, 128 | { 129 | "cell_type": "code", 130 | "execution_count": null, 131 | "metadata": {}, 132 | "outputs": [], 133 | "source": [ 134 | "function terminal_cost(x)\n", 135 | " return 0.5*(x-xgoal)'*Qn*(x-xgoal)\n", 136 | "end" 137 | ] 138 | }, 139 | { 140 | "cell_type": "code", 141 | "execution_count": null, 142 | "metadata": {}, 143 | "outputs": [], 144 | "source": [ 145 | "function cost(xtraj,utraj)\n", 146 | " J = 0.0\n", 147 | " for k = 1:(Nt-1)\n", 148 | " J += stage_cost(xtraj[:,k],utraj[k])\n", 149 | " end\n", 150 | " J += terminal_cost(xtraj[:,Nt])\n", 151 | " return J\n", 152 | "end" 153 | ] 154 | }, 155 | { 156 | "cell_type": "code", 157 | "execution_count": null, 158 | "metadata": {}, 159 | "outputs": [], 160 | "source": [ 161 | "function backward_pass!(p,P,d,K)\n", 162 | " \n", 163 | " ΔJ = 0.0\n", 164 | " p[:,Nt] .= Qn*(xtraj[:,Nt]-xgoal)\n", 165 | " P[:,:,Nt] .= Qn\n", 166 | " \n", 167 | " for k = (Nt-1):-1:1\n", 168 | " #Calculate derivatives\n", 169 | " q = Q*(xtraj[:,k]-xgoal)\n", 170 | " r = R*utraj[k]\n", 171 | " \n", 172 | " A = dfdx(xtraj[:,k], utraj[k])\n", 173 | " B = dfdu(xtraj[:,k], utraj[k])\n", 174 | " \n", 175 | " gx = q + A'*p[:,k+1]\n", 176 | " gu = r + B'*p[:,k+1]\n", 177 | " \n", 178 | " #iLQR (Gauss-Newton) version\n", 179 | " Gxx = Q + A'*P[:,:,k+1]*A\n", 180 | " Guu = R + B'*P[:,:,k+1]*B\n", 181 | " Gxu = A'*P[:,:,k+1]*B\n", 182 | " Gux = B'*P[:,:,k+1]*A\n", 183 | " \n", 184 | " #DDP (full Newton) version\n", 185 | " #Ax = dAdx(xtraj[:,k], utraj[k])\n", 186 | " #Bx = dBdx(xtraj[:,k], utraj[k])\n", 187 | " #Au = dAdu(xtraj[:,k], utraj[k])\n", 188 | " #Bu = dBdu(xtraj[:,k], utraj[k])\n", 189 | " #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 += β*I\n", 197 | " # Guu += β*I\n", 198 | " # β = 2*β\n", 199 | " # display(\"regularizing G\")\n", 200 | " # display(β)\n", 201 | " #end\n", 202 | " \n", 203 | " d[k] = Guu\\gu\n", 204 | " K[:,:,k] .= Guu\\Gux\n", 205 | " \n", 206 | " #p[:,k] .= dropdims(gx - K[:,:,k]'*gu + K[:,:,k]'*Guu*d[k] - Gxu*d[k], dims=2)\n", 207 | " p[:,k] = gx - K[:,:,k]'*gu + K[:,:,k]'*Guu*d[k] - Gxu*d[k]\n", 208 | " P[:,:,k] .= Gxx + K[:,:,k]'*Guu*K[:,:,k] - Gxu*K[:,:,k] - K[:,:,k]'*Gux\n", 209 | " \n", 210 | " ΔJ += gu'*d[k]\n", 211 | " end\n", 212 | " \n", 213 | " return ΔJ\n", 214 | "end" 215 | ] 216 | }, 217 | { 218 | "cell_type": "code", 219 | "execution_count": null, 220 | "metadata": {}, 221 | "outputs": [], 222 | "source": [ 223 | "#Initial guess\n", 224 | "x0 = [-pi/2; 0; 0; 0]\n", 225 | "xgoal = [pi/2; 0; 0; 0]\n", 226 | "xtraj = kron(ones(1,Nt), x0)\n", 227 | "utraj = randn(Nt-1);\n", 228 | "#f = jldopen(\"guess.jld2\", \"r\")\n", 229 | "#utraj = f[\"utraj\"];" 230 | ] 231 | }, 232 | { 233 | "cell_type": "code", 234 | "execution_count": null, 235 | "metadata": {}, 236 | "outputs": [], 237 | "source": [ 238 | "#Initial Rollout\n", 239 | "for k = 1:(Nt-1)\n", 240 | " xtraj[:,k+1] .= dynamics_rk4(xtraj[:,k],utraj[k])\n", 241 | "end\n", 242 | "J = cost(xtraj,utraj)" 243 | ] 244 | }, 245 | { 246 | "cell_type": "code", 247 | "execution_count": null, 248 | "metadata": {}, 249 | "outputs": [], 250 | "source": [ 251 | "#DDP Algorithm\n", 252 | "using Printf\n", 253 | "p = ones(Nx,Nt)\n", 254 | "P = zeros(Nx,Nx,Nt)\n", 255 | "d = ones(Nt-1)\n", 256 | "K = zeros(Nu,Nx,Nt-1)\n", 257 | "ΔJ = 0.0\n", 258 | "\n", 259 | "xn = zeros(Nx,Nt)\n", 260 | "un = zeros(Nt-1)\n", 261 | "\n", 262 | "gx = zeros(Nx)\n", 263 | "gu = 0.0\n", 264 | "Gxx = zeros(Nx,Nx)\n", 265 | "Guu = 0.0\n", 266 | "Gxu = zeros(Nx)\n", 267 | "Gux = zeros(Nx)\n", 268 | "\n", 269 | "iter = 0\n", 270 | "while maximum(abs.(d[:])) > 1e-3\n", 271 | " iter += 1 \n", 272 | " \n", 273 | " #Backward Pass\n", 274 | " ΔJ = backward_pass!(p,P,d,K)\n", 275 | "\n", 276 | " #Forward rollout with line search\n", 277 | " xn[:,1] = xtraj[:,1]\n", 278 | " α = 1.0\n", 279 | " for k = 1:(Nt-1)\n", 280 | " un[k] = utraj[k] - α*d[k] - dot(K[:,:,k],xn[:,k]-xtraj[:,k])\n", 281 | " xn[:,k+1] .= dynamics_rk4(xn[:,k],un[k])\n", 282 | " end\n", 283 | " Jn = cost(xn,un)\n", 284 | " \n", 285 | " while isnan(Jn) || Jn > (J - 1e-2*α*ΔJ)\n", 286 | " α = 0.5*α\n", 287 | " for k = 1:(Nt-1)\n", 288 | " un[k] = utraj[k] - α*d[k] - dot(K[:,:,k],xn[:,k]-xtraj[:,k])\n", 289 | " xn[:,k+1] .= dynamics_rk4(xn[:,k],un[k])\n", 290 | " end\n", 291 | " Jn = cost(xn,un)\n", 292 | " end\n", 293 | "\n", 294 | " # logging\n", 295 | " if rem(iter - 1, 100) == 0\n", 296 | " @printf \"iter J ΔJ |d| α \\n\"\n", 297 | " @printf \"---------------------------------------------------\\n\"\n", 298 | " end\n", 299 | " if rem(iter - 1, 10) == 0 \n", 300 | " @printf(\"%3d %10.3e %9.2e %9.2e %6.4f \\n\",\n", 301 | " iter, J, ΔJ, maximum(abs.(d[:])), α)\n", 302 | " end\n", 303 | " \n", 304 | " J = Jn\n", 305 | " xtraj .= xn\n", 306 | " utraj .= un\n", 307 | "end" 308 | ] 309 | }, 310 | { 311 | "cell_type": "code", 312 | "execution_count": null, 313 | "metadata": {}, 314 | "outputs": [], 315 | "source": [ 316 | "plot(thist,xtraj[1,:])\n", 317 | "plot(thist,xtraj[2,:])" 318 | ] 319 | }, 320 | { 321 | "cell_type": "code", 322 | "execution_count": null, 323 | "metadata": {}, 324 | "outputs": [], 325 | "source": [ 326 | "plot(thist[1:Nt-1],utraj)" 327 | ] 328 | }, 329 | { 330 | "cell_type": "code", 331 | "execution_count": null, 332 | "metadata": {}, 333 | "outputs": [], 334 | "source": [ 335 | "# Acrobot (doublependulum)\n", 336 | "import MeshCat as mc\n", 337 | "using Colors\n", 338 | "\n", 339 | "function RotX(alpha)\n", 340 | " c, s = cos(alpha), sin(alpha)\n", 341 | " [1 0 0; 0 c -s; 0 s c]\n", 342 | "end\n", 343 | "\n", 344 | "function create_acrobot!(vis, color=colorant\"blue\", thick=0.05)\n", 345 | " l1,l2 = [1.,1.]\n", 346 | " hinge = mc.Cylinder(mc.Point(-0.05,0,0), mc.Point(0.05,0,0), 0.05)\n", 347 | " dim1 = mc.Vec(thick, thick, l1)\n", 348 | " link1 = mc.HyperRectangle(mc.Vec(-thick/2,-thick/2,0),dim1)\n", 349 | " dim2 = mc.Vec(thick, thick, l2)\n", 350 | " link2 = mc.HyperRectangle(mc.Vec(-thick/2,-thick/2,0),dim2)\n", 351 | " mat1 = mc.MeshPhongMaterial(color=colorant\"grey\")\n", 352 | " mat2 = mc.MeshPhongMaterial(color=color)\n", 353 | " mc.setobject!(vis[\"base\"], hinge, mat1) \n", 354 | " mc.setobject!(vis[\"link1\"], link1, mat2) \n", 355 | " mc.setobject!(vis[\"link1\",\"joint\"], hinge, mat1) \n", 356 | " mc.setobject!(vis[\"link1\",\"link2\"], link2, mat2) \n", 357 | " mc.settransform!(vis[\"link1\",\"link2\"], mc.Translation(0,0,l1))\n", 358 | " mc.settransform!(vis[\"link1\",\"joint\"], mc.Translation(0,0,l1))\n", 359 | "end\n", 360 | "\n", 361 | "function update_acro_pose!(vis, x)\n", 362 | " l1, l2 = [1, 1.]\n", 363 | " mc.settransform!(vis[\"robot\",\"link1\"], mc.LinearMap(RotX(x[1]-pi/2)))\n", 364 | " mc.settransform!(vis[\"robot\",\"link1\",\"link2\"], mc.compose(mc.Translation(0,0,l1), mc.LinearMap(RotX(x[2]))))\n", 365 | "end\n", 366 | "\n", 367 | "function animate_acrobot(X, dt)\n", 368 | " vis = mc.Visualizer()\n", 369 | " create_acrobot!(vis[\"robot\"])\n", 370 | " anim = mc.Animation(vis; fps=floor(Int,1/dt))\n", 371 | " for k = 1:length(X)\n", 372 | " mc.atframe(anim, k) do\n", 373 | " update_acro_pose!(vis,X[k])\n", 374 | " end\n", 375 | " end\n", 376 | " mc.setanimation!(vis, anim)\n", 377 | " return mc.render(vis)\n", 378 | "end" 379 | ] 380 | }, 381 | { 382 | "cell_type": "code", 383 | "execution_count": null, 384 | "metadata": {}, 385 | "outputs": [], 386 | "source": [ 387 | "X1 = [Vector(x) for x in eachcol(xtraj)];\n", 388 | "animate_acrobot(X1, h)" 389 | ] 390 | }, 391 | { 392 | "cell_type": "code", 393 | "execution_count": null, 394 | "metadata": {}, 395 | "outputs": [], 396 | "source": [] 397 | }, 398 | { 399 | "cell_type": "code", 400 | "execution_count": null, 401 | "metadata": {}, 402 | "outputs": [], 403 | "source": [] 404 | } 405 | ], 406 | "metadata": { 407 | "kernelspec": { 408 | "display_name": "Julia 1.10.8", 409 | "language": "julia", 410 | "name": "julia-1.10" 411 | }, 412 | "language_info": { 413 | "file_extension": ".jl", 414 | "mimetype": "application/julia", 415 | "name": "julia", 416 | "version": "1.10.8" 417 | } 418 | }, 419 | "nbformat": 4, 420 | "nbformat_minor": 4 421 | } 422 | -------------------------------------------------------------------------------- /Lecture 12/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\n", 24 | "using JLD2" 25 | ] 26 | }, 27 | { 28 | "cell_type": "code", 29 | "execution_count": null, 30 | "metadata": {}, 31 | "outputs": [], 32 | "source": [ 33 | "#Cartpole Dynamics\n", 34 | "a = RobotZoo.Cartpole()\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 = 5.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); 1.0*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])\n", 173 | " B = dfdu(xtraj[:,k], utraj[k])\n", 174 | " \n", 175 | " gx = q + A'*p[:,k+1]\n", 176 | " gu = r + B'*p[:,k+1]\n", 177 | " \n", 178 | " #iLQR (Gauss-Newton) version\n", 179 | " Gxx = Q + A'*P[:,:,k+1]*A\n", 180 | " Guu = R + B'*P[:,:,k+1]*B\n", 181 | " Gxu = A'*P[:,:,k+1]*B\n", 182 | " Gux = B'*P[:,:,k+1]*A\n", 183 | " \n", 184 | " #DDP (full Newton) version\n", 185 | " #Ax = dAdx(xtraj[:,k], utraj[k])\n", 186 | " #Bx = dBdx(xtraj[:,k], utraj[k])\n", 187 | " #Au = dAdu(xtraj[:,k], utraj[k])\n", 188 | " #Bu = dBdu(xtraj[:,k], utraj[k])\n", 189 | " #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 += β*I\n", 197 | " # Guu += β*I\n", 198 | " # β = 2*β\n", 199 | " # display(\"regularizing G\")\n", 200 | " # display(β)\n", 201 | " #end\n", 202 | " \n", 203 | " d[k] = Guu\\gu\n", 204 | " K[:,:,k] .= Guu\\Gux\n", 205 | " \n", 206 | " #p[:,k] .= dropdims(gx - K[:,:,k]'*gu + K[:,:,k]'*Guu*d[k] - Gxu*d[k], dims=2)\n", 207 | " p[:,k] = gx - K[:,:,k]'*gu + K[:,:,k]'*Guu*d[k] - Gxu*d[k]\n", 208 | " P[:,:,k] .= Gxx + K[:,:,k]'*Guu*K[:,:,k] - Gxu*K[:,:,k] - K[:,:,k]'*Gux\n", 209 | " \n", 210 | " ΔJ += gu'*d[k]\n", 211 | " end\n", 212 | " \n", 213 | " return ΔJ\n", 214 | "end" 215 | ] 216 | }, 217 | { 218 | "cell_type": "code", 219 | "execution_count": null, 220 | "metadata": {}, 221 | "outputs": [], 222 | "source": [ 223 | "#Initial guess\n", 224 | "x0 = [0; 0; 0; 0]\n", 225 | "xgoal = [0, pi, 0, 0]\n", 226 | "xtraj = kron(ones(1,Nt), x0)\n", 227 | "utraj = randn(Nt-1);" 228 | ] 229 | }, 230 | { 231 | "cell_type": "code", 232 | "execution_count": null, 233 | "metadata": {}, 234 | "outputs": [], 235 | "source": [ 236 | "#Initial Rollout\n", 237 | "for k = 1:(Nt-1)\n", 238 | " xtraj[:,k+1] .= dynamics_rk4(xtraj[:,k],utraj[k])\n", 239 | "end\n", 240 | "J = cost(xtraj,utraj)" 241 | ] 242 | }, 243 | { 244 | "cell_type": "code", 245 | "execution_count": null, 246 | "metadata": {}, 247 | "outputs": [], 248 | "source": [ 249 | "#DDP Algorithm\n", 250 | "using Printf\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 = 0.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 | " for k = 1:(Nt-1)\n", 278 | " un[k] = utraj[k] - α*d[k] - dot(K[:,:,k],xn[:,k]-xtraj[:,k])\n", 279 | " xn[:,k+1] .= dynamics_rk4(xn[:,k],un[k])\n", 280 | " end\n", 281 | " Jn = cost(xn,un)\n", 282 | " \n", 283 | " while isnan(Jn) || Jn > (J - 1e-2*α*ΔJ)\n", 284 | " α = 0.5*α\n", 285 | " for k = 1:(Nt-1)\n", 286 | " un[k] = utraj[k] - α*d[k] - dot(K[:,:,k],xn[:,k]-xtraj[:,k])\n", 287 | " xn[:,k+1] .= dynamics_rk4(xn[:,k],un[k])\n", 288 | " end\n", 289 | " Jn = cost(xn,un)\n", 290 | " end\n", 291 | "\n", 292 | " # logging\n", 293 | " if rem(iter - 1, 100) == 0\n", 294 | " @printf \"iter J ΔJ |d| α \\n\"\n", 295 | " @printf \"---------------------------------------------------\\n\"\n", 296 | " end\n", 297 | " if rem(iter - 1, 10) == 0 \n", 298 | " @printf(\"%3d %10.3e %9.2e %9.2e %6.4f \\n\",\n", 299 | " iter, J, ΔJ, maximum(abs.(d[:])), α)\n", 300 | " end\n", 301 | " \n", 302 | " J = Jn\n", 303 | " xtraj .= xn\n", 304 | " utraj .= un\n", 305 | "end" 306 | ] 307 | }, 308 | { 309 | "cell_type": "code", 310 | "execution_count": null, 311 | "metadata": {}, 312 | "outputs": [], 313 | "source": [ 314 | "iter" 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(vis; fps=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 | "cell_type": "code", 399 | "execution_count": null, 400 | "metadata": {}, 401 | "outputs": [], 402 | "source": [] 403 | } 404 | ], 405 | "metadata": { 406 | "kernelspec": { 407 | "display_name": "Julia 1.10.8", 408 | "language": "julia", 409 | "name": "julia-1.10" 410 | }, 411 | "language_info": { 412 | "file_extension": ".jl", 413 | "mimetype": "application/julia", 414 | "name": "julia", 415 | "version": "1.10.8" 416 | } 417 | }, 418 | "nbformat": 4, 419 | "nbformat_minor": 4 420 | } 421 | -------------------------------------------------------------------------------- /Lecture 12/guess.jld2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 12/guess.jld2 -------------------------------------------------------------------------------- /Lecture 13/Lecture 13.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 13/Lecture 13.pdf -------------------------------------------------------------------------------- /Lecture 13/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | MathOptInterface = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" 3 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 4 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 5 | -------------------------------------------------------------------------------- /Lecture 13/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-4,c_tol=1.0e-4,max_iter=1000)\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)" 330 | ] 331 | }, 332 | { 333 | "cell_type": "code", 334 | "execution_count": null, 335 | "metadata": {}, 336 | "outputs": [], 337 | "source": [ 338 | "z_sol = solve(z0,prob) # solve" 339 | ] 340 | }, 341 | { 342 | "cell_type": "code", 343 | "execution_count": null, 344 | "metadata": {}, 345 | "outputs": [], 346 | "source": [ 347 | "ztraj = reshape(z_sol,Nx+Nu,Nt)\n", 348 | "xtraj = ztraj[1:Nx,:]\n", 349 | "utraj = ztraj[(Nx+1):(Nx+Nu),:];" 350 | ] 351 | }, 352 | { 353 | "cell_type": "code", 354 | "execution_count": null, 355 | "metadata": {}, 356 | "outputs": [], 357 | "source": [ 358 | "plot(thist,xtraj[1,:])\n", 359 | "plot(thist,xtraj[2,:])\n", 360 | "plot(thist,xguess[1,:])\n", 361 | "plot(thist,xguess[2,:])" 362 | ] 363 | }, 364 | { 365 | "cell_type": "code", 366 | "execution_count": null, 367 | "metadata": {}, 368 | "outputs": [], 369 | "source": [ 370 | "plot(thist,xtraj[3,:])\n", 371 | "plot(thist,xtraj[4,:])" 372 | ] 373 | }, 374 | { 375 | "cell_type": "code", 376 | "execution_count": null, 377 | "metadata": {}, 378 | "outputs": [], 379 | "source": [ 380 | "plot(thist,utraj[1,:])" 381 | ] 382 | }, 383 | { 384 | "cell_type": "code", 385 | "execution_count": null, 386 | "metadata": {}, 387 | "outputs": [], 388 | "source": [ 389 | "# Acrobot (doublependulum)\n", 390 | "import MeshCat as mc\n", 391 | "using Colors\n", 392 | "\n", 393 | "function RotX(alpha)\n", 394 | " c, s = cos(alpha), sin(alpha)\n", 395 | " [1 0 0; 0 c -s; 0 s c]\n", 396 | "end\n", 397 | "\n", 398 | "function create_acrobot!(vis, color=colorant\"blue\", thick=0.05)\n", 399 | " l1,l2 = [1.,1.]\n", 400 | " hinge = mc.Cylinder(mc.Point(-0.05,0,0), mc.Point(0.05,0,0), 0.05)\n", 401 | " dim1 = mc.Vec(thick, thick, l1)\n", 402 | " link1 = mc.HyperRectangle(mc.Vec(-thick/2,-thick/2,0),dim1)\n", 403 | " dim2 = mc.Vec(thick, thick, l2)\n", 404 | " link2 = mc.HyperRectangle(mc.Vec(-thick/2,-thick/2,0),dim2)\n", 405 | " mat1 = mc.MeshPhongMaterial(color=colorant\"grey\")\n", 406 | " mat2 = mc.MeshPhongMaterial(color=color)\n", 407 | " mc.setobject!(vis[\"base\"], hinge, mat1) \n", 408 | " mc.setobject!(vis[\"link1\"], link1, mat2) \n", 409 | " mc.setobject!(vis[\"link1\",\"joint\"], hinge, mat1) \n", 410 | " mc.setobject!(vis[\"link1\",\"link2\"], link2, mat2) \n", 411 | " mc.settransform!(vis[\"link1\",\"link2\"], mc.Translation(0,0,l1))\n", 412 | " mc.settransform!(vis[\"link1\",\"joint\"], mc.Translation(0,0,l1))\n", 413 | "end\n", 414 | "\n", 415 | "function update_acro_pose!(vis, x)\n", 416 | " l1, l2 = [1, 1.]\n", 417 | " mc.settransform!(vis[\"robot\",\"link1\"], mc.LinearMap(RotX(x[1]-pi/2)))\n", 418 | " mc.settransform!(vis[\"robot\",\"link1\",\"link2\"], mc.compose(mc.Translation(0,0,l1), mc.LinearMap(RotX(x[2]))))\n", 419 | "end\n", 420 | "\n", 421 | "function animate_acrobot(X, dt)\n", 422 | " vis = mc.Visualizer()\n", 423 | " create_acrobot!(vis[\"robot\"])\n", 424 | " anim = mc.Animation(vis, fps=floor(Int,1/dt))\n", 425 | " for k = 1:length(X)\n", 426 | " mc.atframe(anim, k) do\n", 427 | " update_acro_pose!(vis,X[k])\n", 428 | " end\n", 429 | " end\n", 430 | " mc.setanimation!(vis, anim)\n", 431 | " return mc.render(vis)\n", 432 | "end" 433 | ] 434 | }, 435 | { 436 | "cell_type": "code", 437 | "execution_count": null, 438 | "metadata": {}, 439 | "outputs": [], 440 | "source": [ 441 | "X1 = [Vector(x) for x in eachcol(xtraj)];\n", 442 | "animate_acrobot(X1, h)" 443 | ] 444 | }, 445 | { 446 | "cell_type": "code", 447 | "execution_count": null, 448 | "metadata": {}, 449 | "outputs": [], 450 | "source": [] 451 | }, 452 | { 453 | "cell_type": "markdown", 454 | "metadata": {}, 455 | "source": [] 456 | } 457 | ], 458 | "metadata": { 459 | "kernelspec": { 460 | "display_name": "Julia 1.10.8", 461 | "language": "julia", 462 | "name": "julia-1.10" 463 | }, 464 | "language_info": { 465 | "file_extension": ".jl", 466 | "mimetype": "application/julia", 467 | "name": "julia", 468 | "version": "1.10.8" 469 | } 470 | }, 471 | "nbformat": 4, 472 | "nbformat_minor": 4 473 | } 474 | -------------------------------------------------------------------------------- /Lecture 14/Lecture 14.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 14/Lecture 14.pdf -------------------------------------------------------------------------------- /Lecture 14/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | julia_version = "1.10.8" 4 | manifest_format = "2.0" 5 | project_hash = "da39a3ee5e6b4b0d3255bfef95601890afd80709" 6 | 7 | [deps] 8 | -------------------------------------------------------------------------------- /Lecture 14/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | -------------------------------------------------------------------------------- /Lecture 14/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 | "Q(qk)'*Q(qk)" 270 | ] 271 | }, 272 | { 273 | "cell_type": "code", 274 | "execution_count": null, 275 | "id": "f055f150-ba90-4d42-8f2b-07a3e9de32dc", 276 | "metadata": {}, 277 | "outputs": [], 278 | "source": [] 279 | } 280 | ], 281 | "metadata": { 282 | "kernelspec": { 283 | "display_name": "Julia 1.10.8", 284 | "language": "julia", 285 | "name": "julia-1.10" 286 | }, 287 | "language_info": { 288 | "file_extension": ".jl", 289 | "mimetype": "application/julia", 290 | "name": "julia", 291 | "version": "1.10.8" 292 | } 293 | }, 294 | "nbformat": 4, 295 | "nbformat_minor": 5 296 | } 297 | -------------------------------------------------------------------------------- /Lecture 15/Lecture 15.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 15/Lecture 15.pdf -------------------------------------------------------------------------------- /Lecture 15/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | julia_version = "1.10.8" 4 | manifest_format = "2.0" 5 | project_hash = "da39a3ee5e6b4b0d3255bfef95601890afd80709" 6 | 7 | [deps] 8 | -------------------------------------------------------------------------------- /Lecture 15/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.10.8", 191 | "language": "julia", 192 | "name": "julia-1.10" 193 | }, 194 | "language_info": { 195 | "file_extension": ".jl", 196 | "mimetype": "application/julia", 197 | "name": "julia", 198 | "version": "1.10.8" 199 | } 200 | }, 201 | "nbformat": 4, 202 | "nbformat_minor": 4 203 | } 204 | -------------------------------------------------------------------------------- /Lecture 16/Lecture 16.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 16/Lecture 16.pdf -------------------------------------------------------------------------------- /Lecture 16/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 3 | TrajOptPlots = "7770976a-8dee-4930-bf39-a1782fd21ce6" 4 | -------------------------------------------------------------------------------- /Lecture 16/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 | "Ã" 211 | ] 212 | }, 213 | { 214 | "cell_type": "code", 215 | "execution_count": null, 216 | "metadata": {}, 217 | "outputs": [], 218 | "source": [ 219 | "#Controlability Matrix\n", 220 | "C = B̃\n", 221 | "for k = 1:(Nx-1)\n", 222 | " C = [C Ã*C[:,end-(Nu-1):end]]\n", 223 | "end" 224 | ] 225 | }, 226 | { 227 | "cell_type": "code", 228 | "execution_count": null, 229 | "metadata": {}, 230 | "outputs": [], 231 | "source": [ 232 | "rank(C) #Reduced system is controllable" 233 | ] 234 | }, 235 | { 236 | "cell_type": "code", 237 | "execution_count": null, 238 | "metadata": {}, 239 | "outputs": [], 240 | "source": [ 241 | "# Cost weights\n", 242 | "Q = Array(I(Nx̃));\n", 243 | "R = Array(.1*I(Nu));" 244 | ] 245 | }, 246 | { 247 | "cell_type": "code", 248 | "execution_count": null, 249 | "metadata": {}, 250 | "outputs": [], 251 | "source": [ 252 | "#LQR Controller\n", 253 | "K = dlqr(Ã,B̃,Q,R)" 254 | ] 255 | }, 256 | { 257 | "cell_type": "code", 258 | "execution_count": null, 259 | "metadata": {}, 260 | "outputs": [], 261 | "source": [ 262 | "#Feedback controller\n", 263 | "function controller(x)\n", 264 | " \n", 265 | " q0 = x0[4:7]\n", 266 | " q = x[4:7]\n", 267 | " ϕ = qtorp(L(q0)'*q)\n", 268 | " \n", 269 | " Δx̃ = [x[1:3]-r0; ϕ; x[8:10]-v0; x[11:13]-ω0]\n", 270 | " \n", 271 | " u = uhover - K*Δx̃\n", 272 | "end" 273 | ] 274 | }, 275 | { 276 | "cell_type": "code", 277 | "execution_count": null, 278 | "metadata": {}, 279 | "outputs": [], 280 | "source": [ 281 | "#Simulation\n", 282 | "uhist = zeros(Nu,Nt)\n", 283 | "xhist = zeros(Nx,Nt)\n", 284 | "xhist[:,1] = [r0+randn(3); L(q0)*rptoq([1; 0; 0]); v0; ω0]\n", 285 | "for k = 1:(Nt-1)\n", 286 | " uhist[:,k] = controller(xhist[:,k])\n", 287 | " xhist[:,k+1] = quad_dynamics_rk4(xhist[:,k],uhist[:,k])\n", 288 | "end" 289 | ] 290 | }, 291 | { 292 | "cell_type": "code", 293 | "execution_count": null, 294 | "metadata": {}, 295 | "outputs": [], 296 | "source": [ 297 | "import Pkg; Pkg.add(\"RobotZoo\")" 298 | ] 299 | }, 300 | { 301 | "cell_type": "code", 302 | "execution_count": null, 303 | "metadata": { 304 | "scrolled": true 305 | }, 306 | "outputs": [], 307 | "source": [ 308 | "#Set up visualization\n", 309 | "using TrajOptPlots\n", 310 | "using MeshCat\n", 311 | "using StaticArrays\n", 312 | "using RobotZoo:Quadrotor\n", 313 | "\n", 314 | "vis = Visualizer()\n", 315 | "render(vis)" 316 | ] 317 | }, 318 | { 319 | "cell_type": "code", 320 | "execution_count": null, 321 | "metadata": {}, 322 | "outputs": [], 323 | "source": [ 324 | "model = Quadrotor()\n", 325 | "TrajOptPlots.set_mesh!(vis, model)" 326 | ] 327 | }, 328 | { 329 | "cell_type": "code", 330 | "execution_count": null, 331 | "metadata": {}, 332 | "outputs": [], 333 | "source": [ 334 | "X1 = [SVector{13}(x) for x in eachcol(xhist)];\n", 335 | "visualize!(vis, model, thist[end], X1)" 336 | ] 337 | }, 338 | { 339 | "cell_type": "code", 340 | "execution_count": null, 341 | "metadata": {}, 342 | "outputs": [], 343 | "source": [] 344 | } 345 | ], 346 | "metadata": { 347 | "kernelspec": { 348 | "display_name": "Julia 1.10.8", 349 | "language": "julia", 350 | "name": "julia-1.10" 351 | }, 352 | "language_info": { 353 | "file_extension": ".jl", 354 | "mimetype": "application/julia", 355 | "name": "julia", 356 | "version": "1.10.8" 357 | } 358 | }, 359 | "nbformat": 4, 360 | "nbformat_minor": 4 361 | } 362 | -------------------------------------------------------------------------------- /Lecture 17/Lecture 17.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 17/Lecture 17.pdf -------------------------------------------------------------------------------- /Lecture 17/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" 3 | CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298" 4 | ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" 5 | GeometryBasics = "5c1252a2-5f33-56bf-86c9-59e7332b4326" 6 | Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" 7 | MathOptInterface = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" 8 | MeshCat = "283c5d60-a78f-5afe-a0af-af636b173e11" 9 | PyPlot = "d330b81b-6aea-500a-939a-2ce795aea3ee" 10 | Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" 11 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 12 | -------------------------------------------------------------------------------- /Lecture 17/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.10.8", 148 | "language": "julia", 149 | "name": "julia-1.10" 150 | }, 151 | "language_info": { 152 | "file_extension": ".jl", 153 | "mimetype": "application/julia", 154 | "name": "julia", 155 | "version": "1.10.8" 156 | } 157 | }, 158 | "nbformat": 4, 159 | "nbformat_minor": 4 160 | } 161 | -------------------------------------------------------------------------------- /Lecture 18/Lecture 18.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 18/Lecture 18.pdf -------------------------------------------------------------------------------- /Lecture 18/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | Altro = "5dcf52e5-e2fb-48e0-b826-96f46d2e3e73" 3 | ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" 4 | OSQP = "ab2f91bb-94b4-55e3-9ba0-7f65df51de79" 5 | PyPlot = "d330b81b-6aea-500a-939a-2ce795aea3ee" 6 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 7 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 8 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 9 | TrajectoryOptimization = "c79d492b-0548-5874-b488-5a62c1d9d0ca" 10 | -------------------------------------------------------------------------------- /Lecture 18/cartpole-ilc.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using TrajectoryOptimization\n", 19 | "using RobotDynamics\n", 20 | "const RD = RobotDynamics\n", 21 | "import RobotZoo.Cartpole\n", 22 | "using StaticArrays\n", 23 | "using SparseArrays\n", 24 | "using LinearAlgebra\n", 25 | "using ForwardDiff\n", 26 | "using PyPlot\n", 27 | "using OSQP" 28 | ] 29 | }, 30 | { 31 | "cell_type": "code", 32 | "execution_count": null, 33 | "metadata": {}, 34 | "outputs": [], 35 | "source": [ 36 | "model = Cartpole()\n", 37 | "n,m = size(model);" 38 | ] 39 | }, 40 | { 41 | "cell_type": "code", 42 | "execution_count": null, 43 | "metadata": {}, 44 | "outputs": [], 45 | "source": [ 46 | "N = 101\n", 47 | "Tf = 5.\n", 48 | "h = Tf/(N-1)" 49 | ] 50 | }, 51 | { 52 | "cell_type": "code", 53 | "execution_count": null, 54 | "metadata": {}, 55 | "outputs": [], 56 | "source": [ 57 | "x0 = @SVector zeros(n)\n", 58 | "xf = @SVector [0, pi, 0, 0]; # i.e. swing up" 59 | ] 60 | }, 61 | { 62 | "cell_type": "code", 63 | "execution_count": null, 64 | "metadata": {}, 65 | "outputs": [], 66 | "source": [ 67 | "# Set up\n", 68 | "Q = 1.0*Diagonal(@SVector ones(n))\n", 69 | "Qf = 100.0*Diagonal(@SVector ones(n))\n", 70 | "R = 0.1*Diagonal(@SVector ones(m))\n", 71 | "obj = LQRObjective(Q,R,Qf,xf,N);" 72 | ] 73 | }, 74 | { 75 | "cell_type": "code", 76 | "execution_count": null, 77 | "metadata": {}, 78 | "outputs": [], 79 | "source": [ 80 | "# Create Empty ConstraintList\n", 81 | "conSet = ConstraintList(n,m,N)\n", 82 | "\n", 83 | "# Control Bounds\n", 84 | "u_bnd = 5.0\n", 85 | "bnd = BoundConstraint(n,m, u_min=-u_bnd, u_max=u_bnd)\n", 86 | "add_constraint!(conSet, bnd, 1:N-1)\n", 87 | "\n", 88 | "# Goal Constraint\n", 89 | "goal = GoalConstraint(xf)\n", 90 | "add_constraint!(conSet, goal, (N-5):N)" 91 | ] 92 | }, 93 | { 94 | "cell_type": "code", 95 | "execution_count": null, 96 | "metadata": {}, 97 | "outputs": [], 98 | "source": [ 99 | "prob = Problem(model, obj, x0, Tf, xf=xf, constraints=conSet);" 100 | ] 101 | }, 102 | { 103 | "cell_type": "code", 104 | "execution_count": null, 105 | "metadata": {}, 106 | "outputs": [], 107 | "source": [ 108 | "u0 = @SVector fill(0.0,m)\n", 109 | "U0 = [u0 for k = 1:N-1]\n", 110 | "initial_controls!(prob, U0)\n", 111 | "rollout!(prob);" 112 | ] 113 | }, 114 | { 115 | "cell_type": "code", 116 | "execution_count": null, 117 | "metadata": {}, 118 | "outputs": [], 119 | "source": [ 120 | "using Altro\n", 121 | "opts = SolverOptions(\n", 122 | " cost_tolerance_intermediate=1e-3,\n", 123 | " penalty_scaling=10.,\n", 124 | " penalty_initial=1.0\n", 125 | ")\n", 126 | "\n", 127 | "altro = ALTROSolver(prob, opts)\n", 128 | "set_options!(altro, show_summary=true)\n", 129 | "solve!(altro);" 130 | ] 131 | }, 132 | { 133 | "cell_type": "code", 134 | "execution_count": null, 135 | "metadata": {}, 136 | "outputs": [], 137 | "source": [ 138 | "# Extract the solution\n", 139 | "Xopt = states(altro);\n", 140 | "Uopt = controls(altro);\n", 141 | "\n", 142 | "xnom = zeros(n,N)\n", 143 | "for k = 1:N\n", 144 | " xnom[:,k] .= Xopt[k]\n", 145 | "end\n", 146 | "\n", 147 | "unom = zeros(m,N-1)\n", 148 | "utraj = zeros(m,N-1)\n", 149 | "for k = 1:N-1\n", 150 | " unom[:,k] .= Uopt[k]\n", 151 | " utraj[:,k] .= Uopt[k]\n", 152 | "end" 153 | ] 154 | }, 155 | { 156 | "cell_type": "code", 157 | "execution_count": null, 158 | "metadata": {}, 159 | "outputs": [], 160 | "source": [ 161 | "function dynamics_rk4(x, u)\n", 162 | " #RK4 integration with zero-order hold on u\n", 163 | " f1 = RD.dynamics(model, x, u)\n", 164 | " f2 = RD.dynamics(model, x + 0.5*h*f1, u)\n", 165 | " f3 = RD.dynamics(model, x + 0.5*h*f2, u)\n", 166 | " f4 = RD.dynamics(model, x + h*f3, u)\n", 167 | " return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 168 | "end\n", 169 | "\n", 170 | "#True model with friction\n", 171 | "function true_dynamics(model::Cartpole, x, u)\n", 172 | " #Perturb masses of cart + pole\n", 173 | " mc = model.mc + 0.02\n", 174 | " mp = model.mp - 0.01\n", 175 | " \n", 176 | " #Perturb length of pendulum\n", 177 | " l = model.l + 0.005\n", 178 | " g = model.g\n", 179 | "\n", 180 | " q = x[ @SVector [1,2] ]\n", 181 | " qd = x[ @SVector [3,4] ]\n", 182 | "\n", 183 | " s = sin(q[2])\n", 184 | " c = cos(q[2])\n", 185 | "\n", 186 | " H = @SMatrix [mc+mp mp*l*c; mp*l*c mp*l^2]\n", 187 | " C = @SMatrix [0 -mp*qd[2]*l*s; 0 0]\n", 188 | " G = @SVector [0, mp*g*l*s]\n", 189 | " B = @SVector [1, 0]\n", 190 | " \n", 191 | " F = [0.01; 0.01].*tanh.(5.0*qd) #nonlinear friction (≈Coulomb/stiction)\n", 192 | "\n", 193 | " qdd = -H\\(C*qd + F + G - B*u[1])\n", 194 | " return [qd; qdd]\n", 195 | "end\n", 196 | "\n", 197 | "function true_dynamics_rk4(x,u)\n", 198 | " #RK4 integration with zero-order hold on u\n", 199 | " f1 = true_dynamics(model, x, u)\n", 200 | " f2 = true_dynamics(model, x + 0.5*h*f1, u)\n", 201 | " f3 = true_dynamics(model, x + 0.5*h*f2, u)\n", 202 | " f4 = true_dynamics(model, x + h*f3, u)\n", 203 | " return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 204 | "end" 205 | ] 206 | }, 207 | { 208 | "cell_type": "code", 209 | "execution_count": null, 210 | "metadata": {}, 211 | "outputs": [], 212 | "source": [ 213 | "#Compute A and B matrices using nominal dynamics + trajectory\n", 214 | "A = zeros(n,n,N-1)\n", 215 | "B = zeros(n,m,N-1)\n", 216 | "for k = 1:(N-1)\n", 217 | " A[:,:,k] .= ForwardDiff.jacobian(x->dynamics_rk4(x,Uopt[k]),Xopt[k])\n", 218 | " B[:,:,k] .= ForwardDiff.jacobian(u->dynamics_rk4(Xopt[k],u),Uopt[k])\n", 219 | "end" 220 | ] 221 | }, 222 | { 223 | "cell_type": "code", 224 | "execution_count": null, 225 | "metadata": {}, 226 | "outputs": [], 227 | "source": [ 228 | "#Compute LQR Tracking Controller\n", 229 | "P = zeros(n,n,N)\n", 230 | "K = zeros(m,n,N-1)\n", 231 | "P[:,:,N] = Qf\n", 232 | "for k = (N-1):-1:1\n", 233 | " K[:,:,k] .= (R + B[:,:,k]'*P[:,:,k+1]*B[:,:,k])\\(B[:,:,k]'*P[:,:,k+1]*A[:,:,k])\n", 234 | " P[:,:,k] .= Q + K[:,:,k]'*R*K[:,:,k] + (A[:,:,k]-B[:,:,k]*K[:,:,k])'*P[:,:,k+1]*(A[:,:,k]-B[:,:,k]*K[:,:,k])\n", 235 | "end" 236 | ] 237 | }, 238 | { 239 | "cell_type": "code", 240 | "execution_count": null, 241 | "metadata": {}, 242 | "outputs": [], 243 | "source": [ 244 | "#Rollout with true dynamics\n", 245 | "xtraj = zeros(n,N)\n", 246 | "for k = 1:(N-1)\n", 247 | " #utraj[:,k] .= unom[:,k] - K[:,:,k]*(xtraj[:,k]-xnom[:,k]) #with LQR tracking controller\n", 248 | " #utraj[:,k] .= min.(u_bnd, max.(-u_bnd, utraj[:,k])) #clip values within torque limits\n", 249 | " xtraj[:,k+1] .= true_dynamics_rk4(xtraj[:,k], utraj[:,k])\n", 250 | "end" 251 | ] 252 | }, 253 | { 254 | "cell_type": "code", 255 | "execution_count": null, 256 | "metadata": {}, 257 | "outputs": [], 258 | "source": [ 259 | "plot(xtraj[1,:])\n", 260 | "plot(xnom[1,:])" 261 | ] 262 | }, 263 | { 264 | "cell_type": "code", 265 | "execution_count": null, 266 | "metadata": {}, 267 | "outputs": [], 268 | "source": [ 269 | "plot(xtraj[2,:])\n", 270 | "plot(xnom[2,:])" 271 | ] 272 | }, 273 | { 274 | "cell_type": "code", 275 | "execution_count": null, 276 | "metadata": {}, 277 | "outputs": [], 278 | "source": [ 279 | "plot(utraj[1,:])\n", 280 | "plot(Uopt[:])" 281 | ] 282 | }, 283 | { 284 | "cell_type": "code", 285 | "execution_count": null, 286 | "metadata": {}, 287 | "outputs": [], 288 | "source": [ 289 | "#Build matrices for ILC QP\n", 290 | "Nh = N\n", 291 | "#Cost\n", 292 | "Qilc = sparse(Diagonal([0.01; 0; 1.0; 0]))\n", 293 | "Rilc = sparse(Diagonal([.1]))\n", 294 | "H = blockdiag(kron(I(Nh-2), blockdiag(Rilc, Qilc)), Rilc, sparse(Qf))\n", 295 | "q = zeros((n+m)*(Nh-1))\n", 296 | "for k = 1:(Nh-2)\n", 297 | " q[(k-1)*(m+n) .+ (1:(m+n))] .= [0.0; Qilc*(xtraj[:,k+1]-Xopt[k+1])]\n", 298 | "end\n", 299 | "q[(Nh-2)*(m+n) .+ (1:(m+n))] .= [0.0; Qf*(xtraj[:,Nh]-Xopt[Nh])]\n", 300 | "\n", 301 | "#Constraints\n", 302 | "U = kron(I(Nh-1), [I zeros(m,n)]) #Matrix that picks out all u\n", 303 | "X = kron(I(Nh-1), [zeros(n,m) I]) #Matrix that picks out all x\n", 304 | "D = spzeros(n*(Nh-1), (n+m)*(Nh-1)) #dynamics constraints\n", 305 | "\n", 306 | "D[1:n,1:m] .= B[:,:,1]\n", 307 | "D[1:n,(m+1):(m+n)] .= -I(n)\n", 308 | "for k = 1:(Nh-2)\n", 309 | " D[(k*n).+(1:n), (m+(k-1)*(n+m)).+(1:(2*n+m))] .= [A[:,:,k+1] B[:,:,k+1] -I]\n", 310 | "end\n", 311 | "\n", 312 | "lb = [zeros(n*(Nh-1)); -u_bnd.-utraj[1:(Nh-1)]]\n", 313 | "ub = [zeros(n*(Nh-1)); u_bnd.-utraj[1:(Nh-1)]]\n", 314 | "\n", 315 | "qp = OSQP.Model()\n", 316 | "OSQP.setup!(qp, P=H, q=q, A=[D; U], l=lb, u=ub, eps_abs=1e-6, eps_rel=1e-6, eps_prim_inf = 1.0e-6, eps_dual_inf = 1.0e-6, polish=1)\n", 317 | "results = OSQP.solve!(qp)\n", 318 | "ztraj = results.x;" 319 | ] 320 | }, 321 | { 322 | "cell_type": "code", 323 | "execution_count": null, 324 | "metadata": {}, 325 | "outputs": [], 326 | "source": [ 327 | "Δu = U*ztraj\n", 328 | "utraj[1:(Nh-1)] .= utraj[1:(Nh-1)]+Δu" 329 | ] 330 | }, 331 | { 332 | "cell_type": "code", 333 | "execution_count": null, 334 | "metadata": {}, 335 | "outputs": [], 336 | "source": [] 337 | }, 338 | { 339 | "cell_type": "code", 340 | "execution_count": null, 341 | "metadata": {}, 342 | "outputs": [], 343 | "source": [] 344 | } 345 | ], 346 | "metadata": { 347 | "kernelspec": { 348 | "display_name": "Julia 1.10.9", 349 | "language": "julia", 350 | "name": "julia-1.10" 351 | }, 352 | "language_info": { 353 | "file_extension": ".jl", 354 | "mimetype": "application/julia", 355 | "name": "julia", 356 | "version": "1.10.9" 357 | } 358 | }, 359 | "nbformat": 4, 360 | "nbformat_minor": 4 361 | } 362 | -------------------------------------------------------------------------------- /Lecture 19/Lecture 19.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 19/Lecture 19.pdf -------------------------------------------------------------------------------- /Lecture 2/Lecture 2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 2/Lecture 2.pdf -------------------------------------------------------------------------------- /Lecture 2/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | julia_version = "1.10.7" 4 | manifest_format = "2.0" 5 | project_hash = "da39a3ee5e6b4b0d3255bfef95601890afd80709" 6 | 7 | [deps] 8 | -------------------------------------------------------------------------------- /Lecture 2/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | -------------------------------------------------------------------------------- /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, 50, .01)\n", 70 | "plot(t_hist1, x_hist1[1,:])" 71 | ] 72 | }, 73 | { 74 | "cell_type": "code", 75 | "execution_count": null, 76 | "metadata": {}, 77 | "outputs": [], 78 | "source": [ 79 | "function pendulum_euler_Ad(x0, h)\n", 80 | " g = 9.81\n", 81 | " Ad = [1 h; -g*h*cos(x0[1]) 1]\n", 82 | "end" 83 | ] 84 | }, 85 | { 86 | "cell_type": "code", 87 | "execution_count": null, 88 | "metadata": {}, 89 | "outputs": [], 90 | "source": [ 91 | "eigvals(pendulum_euler_Ad(0, 0.001))" 92 | ] 93 | }, 94 | { 95 | "cell_type": "code", 96 | "execution_count": null, 97 | "metadata": {}, 98 | "outputs": [], 99 | "source": [ 100 | "eignorm = zeros(100)\n", 101 | "h = LinRange(0,0.1,100)\n", 102 | "for k = 1:length(eignorm)\n", 103 | " eignorm[k] = max(norm.(eigvals(pendulum_euler_Ad([0;0], h[k])))...)\n", 104 | "end\n", 105 | "plot(h,eignorm)" 106 | ] 107 | }, 108 | { 109 | "cell_type": "code", 110 | "execution_count": null, 111 | "metadata": {}, 112 | "outputs": [], 113 | "source": [ 114 | "function fd_pendulum_rk4(xk, h)\n", 115 | " f1 = pendulum_dynamics(xk)\n", 116 | " f2 = pendulum_dynamics(xk + 0.5*h*f1)\n", 117 | " f3 = pendulum_dynamics(xk + 0.5*h*f2)\n", 118 | " f4 = pendulum_dynamics(xk + h*f3)\n", 119 | " return xk + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 120 | "end" 121 | ] 122 | }, 123 | { 124 | "cell_type": "code", 125 | "execution_count": null, 126 | "metadata": {}, 127 | "outputs": [], 128 | "source": [ 129 | "function pendulum_rk4(fun, x0, Tf, h) \n", 130 | " t = Array(range(0,Tf,step=h))\n", 131 | " \n", 132 | " x_hist = zeros(length(x0),length(t))\n", 133 | " x_hist[:,1] .= x0\n", 134 | " \n", 135 | " for k = 1:(length(t)-1)\n", 136 | " x_hist[:,k+1] .= fd_pendulum_rk4(x_hist[:,k], h)\n", 137 | " end\n", 138 | " \n", 139 | " return x_hist, t\n", 140 | "end" 141 | ] 142 | }, 143 | { 144 | "cell_type": "code", 145 | "execution_count": null, 146 | "metadata": {}, 147 | "outputs": [], 148 | "source": [ 149 | "x0 = [.1; 0]\n", 150 | "x_hist2, t_hist2 = pendulum_rk4(pendulum_dynamics, x0, 100, 0.01)\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.01), [0; 0])\n", 162 | "norm.(eigvals(Ad))" 163 | ] 164 | }, 165 | { 166 | "cell_type": "code", 167 | "execution_count": null, 168 | "metadata": {}, 169 | "outputs": [], 170 | "source": [ 171 | "eignorm = zeros(100)\n", 172 | "h = LinRange(0,1,100)\n", 173 | "for k = 1:length(eignorm)\n", 174 | " eignorm[k] = max(norm.(eigvals(ForwardDiff.jacobian(x -> fd_pendulum_rk4(x, h[k]), [0; 0])))...)\n", 175 | "end\n", 176 | "plot(h,eignorm)" 177 | ] 178 | }, 179 | { 180 | "cell_type": "code", 181 | "execution_count": null, 182 | "metadata": {}, 183 | "outputs": [], 184 | "source": [ 185 | "function pendulum_backward_euler(fun, x0, Tf, dt)\n", 186 | " t = Array(range(0,Tf,step=dt))\n", 187 | " \n", 188 | " x_hist = zeros(length(x0),length(t))\n", 189 | " x_hist[:,1] .= x0\n", 190 | " \n", 191 | " for k = 1:(length(t)-1)\n", 192 | " e = 1\n", 193 | " x_hist[:,k+1] = x_hist[:,k]\n", 194 | " while e > 1e-8\n", 195 | " xn = x_hist[:,k] + dt.*fun(x_hist[:,k+1])\n", 196 | " e = norm(xn - x_hist[:,k+1])\n", 197 | " x_hist[:,k+1] .= xn\n", 198 | " end\n", 199 | " end\n", 200 | " \n", 201 | " return x_hist, t\n", 202 | "end" 203 | ] 204 | }, 205 | { 206 | "cell_type": "code", 207 | "execution_count": null, 208 | "metadata": {}, 209 | "outputs": [], 210 | "source": [ 211 | "x0 = [.1; 0]\n", 212 | "x_hist3, t_hist3 = pendulum_backward_euler(pendulum_dynamics, x0, 10, 0.01)\n", 213 | "plot(t_hist3, x_hist3[1,:])" 214 | ] 215 | }, 216 | { 217 | "cell_type": "code", 218 | "execution_count": null, 219 | "metadata": {}, 220 | "outputs": [], 221 | "source": [] 222 | }, 223 | { 224 | "cell_type": "code", 225 | "execution_count": null, 226 | "metadata": {}, 227 | "outputs": [], 228 | "source": [] 229 | } 230 | ], 231 | "metadata": { 232 | "kernelspec": { 233 | "display_name": "Julia 1.10.7", 234 | "language": "julia", 235 | "name": "julia-1.10" 236 | }, 237 | "language_info": { 238 | "file_extension": ".jl", 239 | "mimetype": "application/julia", 240 | "name": "julia", 241 | "version": "1.10.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/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 20/Lecture 20.pdf -------------------------------------------------------------------------------- /Lecture 21/Lecture 21.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 21/Lecture 21.pdf -------------------------------------------------------------------------------- /Lecture 21/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e" 3 | ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210" 4 | PyPlot = "d330b81b-6aea-500a-939a-2ce795aea3ee" 5 | -------------------------------------------------------------------------------- /Lecture 21/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' + 1e-5*I #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 = 30.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": "b9deaa89-67b8-4d4c-b3da-c794874cca28", 226 | "metadata": {}, 227 | "outputs": [], 228 | "source": [] 229 | } 230 | ], 231 | "metadata": { 232 | "kernelspec": { 233 | "display_name": "Julia 1.10.9", 234 | "language": "julia", 235 | "name": "julia-1.10" 236 | }, 237 | "language_info": { 238 | "file_extension": ".jl", 239 | "mimetype": "application/julia", 240 | "name": "julia", 241 | "version": "1.10.9" 242 | } 243 | }, 244 | "nbformat": 4, 245 | "nbformat_minor": 5 246 | } 247 | -------------------------------------------------------------------------------- /Lecture 22/Lecture 22.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 22/Lecture 22.pdf -------------------------------------------------------------------------------- /Lecture 23/Lecture 23.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 23/Lecture 23.pdf -------------------------------------------------------------------------------- /Lecture 24/Lecture 24.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 24/Lecture 24.pdf -------------------------------------------------------------------------------- /Lecture 24/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" 3 | CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298" 4 | RobotDynamics = "38ceca67-d8d3-44e8-9852-78a5596522e1" 5 | RobotZoo = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" 6 | Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" 7 | StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" 8 | -------------------------------------------------------------------------------- /Lecture 24/oscillator.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "2bd9fa1f-7d12-40eb-88c6-593fe4548e0a", 7 | "metadata": {}, 8 | "outputs": [], 9 | "source": [ 10 | "using LinearAlgebra\n", 11 | "using PyPlot" 12 | ] 13 | }, 14 | { 15 | "cell_type": "code", 16 | "execution_count": null, 17 | "id": "082cf5ec-e3ee-4c06-ba76-34bbd90c452b", 18 | "metadata": {}, 19 | "outputs": [], 20 | "source": [ 21 | "#Damped simple harmonic oscillator\n", 22 | "k = 10.0; #spring constant\n", 23 | "c = 0.2; #damping constant\n", 24 | "h = 0.1; #10 Hz\n", 25 | "A = exp(h*[0 1; -k -c])" 26 | ] 27 | }, 28 | { 29 | "cell_type": "code", 30 | "execution_count": null, 31 | "id": "2624aae4-ef73-42c4-9fa5-67d2d06b5fd8", 32 | "metadata": {}, 33 | "outputs": [], 34 | "source": [ 35 | "T = 50 #5 seconds\n", 36 | "\n", 37 | "x0 = [1.0; 0]\n", 38 | "xtraj = zeros(2,T);\n", 39 | "xtraj[:,1] .= x0\n", 40 | "\n", 41 | "for k = 1:(T-1)\n", 42 | " xtraj[:,k+1] .= A*xtraj[:,k];\n", 43 | "end" 44 | ] 45 | }, 46 | { 47 | "cell_type": "code", 48 | "execution_count": null, 49 | "id": "087159c0-2dbe-4115-82ee-d877ab8537cb", 50 | "metadata": {}, 51 | "outputs": [], 52 | "source": [ 53 | "plot(xtraj[1,:])\n", 54 | "plot(xtraj[2,:])" 55 | ] 56 | }, 57 | { 58 | "cell_type": "code", 59 | "execution_count": null, 60 | "id": "df98dc10-b531-4276-83e2-1e9cee51eed0", 61 | "metadata": {}, 62 | "outputs": [], 63 | "source": [ 64 | "#Generate a bunch of trajectories from random initial conditions\n", 65 | "N = 10\n", 66 | "H = zeros(2*T,N)\n", 67 | "for ℓ = 1:N\n", 68 | " xtraj = zeros(2,T)\n", 69 | " xtraj[:,1] .= randn(2)\n", 70 | " for k = 1:(T-1)\n", 71 | " xtraj[:,k+1] .= A*xtraj[:,k]\n", 72 | " end\n", 73 | " H[:,ℓ] .= xtraj[:]\n", 74 | "end" 75 | ] 76 | }, 77 | { 78 | "cell_type": "code", 79 | "execution_count": null, 80 | "id": "d5a67584-77a3-4eea-90a5-cc527e892c02", 81 | "metadata": {}, 82 | "outputs": [], 83 | "source": [ 84 | "#Pick some new initial condition\n", 85 | "x0 = [-1; 1]\n", 86 | "\n", 87 | "#Simulate with rollout\n", 88 | "xtraj1 = zeros(2,T);\n", 89 | "xtraj1[:,1] .= x0\n", 90 | "\n", 91 | "for k = 1:(T-1)\n", 92 | " xtraj1[:,k+1] .= A*xtraj1[:,k];\n", 93 | "end\n", 94 | "xtraj1" 95 | ] 96 | }, 97 | { 98 | "cell_type": "code", 99 | "execution_count": null, 100 | "id": "57f9935f-b8ca-4194-9c32-d4f59adabdce", 101 | "metadata": {}, 102 | "outputs": [], 103 | "source": [ 104 | "#Generate the same trajectory using the data matrix\n", 105 | "E = [I zeros(2,2*(T-1))]\n", 106 | "w = (H'*E'*E*H)\\(H'*E'*x0)\n", 107 | "z = H*w\n", 108 | "xtraj2 = reshape(z,2,T)" 109 | ] 110 | }, 111 | { 112 | "cell_type": "code", 113 | "execution_count": null, 114 | "id": "1aea7d84-7b8d-4e60-802a-640007c481a7", 115 | "metadata": {}, 116 | "outputs": [], 117 | "source": [ 118 | "#The rollout matches the projection\n", 119 | "plot(xtraj1[1,:])\n", 120 | "plot(xtraj2[1,:])\n", 121 | "xtraj1-xtraj2" 122 | ] 123 | }, 124 | { 125 | "cell_type": "code", 126 | "execution_count": null, 127 | "id": "f2ce4092-a3be-4f8b-85d2-abc82fd3c200", 128 | "metadata": {}, 129 | "outputs": [], 130 | "source": [ 131 | "#Rank of the data matrix = dimension of the data manifold\n", 132 | "rank(H)" 133 | ] 134 | }, 135 | { 136 | "cell_type": "code", 137 | "execution_count": null, 138 | "id": "7f8c0af6-ca83-4adf-b738-ed08d17b948a", 139 | "metadata": {}, 140 | "outputs": [], 141 | "source": [ 142 | "#Do the same thing with only positions y = [1 0]*x\n", 143 | "C = [1.0 0]\n", 144 | "N = 10\n", 145 | "H = zeros(T,N)\n", 146 | "for ℓ = 1:N\n", 147 | " xtraj = zeros(2,T)\n", 148 | " xtraj[:,1] .= randn(2)\n", 149 | " for k = 1:(T-1)\n", 150 | " xtraj[:,k+1] .= A*xtraj[:,k]\n", 151 | " end\n", 152 | " ytraj = C*xtraj;\n", 153 | " H[:,ℓ] .= ytraj[:];\n", 154 | "end" 155 | ] 156 | }, 157 | { 158 | "cell_type": "code", 159 | "execution_count": null, 160 | "id": "9bac02bd-24b6-4efa-8061-fcc2ca64e09d", 161 | "metadata": {}, 162 | "outputs": [], 163 | "source": [ 164 | "rank(H) #rank of data matrix is still the same" 165 | ] 166 | }, 167 | { 168 | "cell_type": "code", 169 | "execution_count": null, 170 | "id": "c8fc973c-973d-4271-bacd-a417853d6cba", 171 | "metadata": {}, 172 | "outputs": [], 173 | "source": [ 174 | "#Pick some new initial condition\n", 175 | "x0 = [1; -1]\n", 176 | "\n", 177 | "#Simulate with rollout\n", 178 | "xtraj1 = zeros(2,T);\n", 179 | "xtraj1[:,1] .= x0\n", 180 | "\n", 181 | "for k = 1:(T-1)\n", 182 | " xtraj1[:,k+1] .= A*xtraj1[:,k];\n", 183 | "end\n", 184 | "ytraj1 = (C*xtraj1)[:]" 185 | ] 186 | }, 187 | { 188 | "cell_type": "code", 189 | "execution_count": null, 190 | "id": "0a04f50e-229c-4f0c-adc2-f8f549c7b476", 191 | "metadata": {}, 192 | "outputs": [], 193 | "source": [ 194 | "#Take the first 2 positions and project onto data manifold\n", 195 | "E = [I zeros(2,T-2)]\n", 196 | "w = (H'*E'*E*H)\\(H'*E'*ytraj1[1:2])\n", 197 | "ytraj2 = H*w" 198 | ] 199 | }, 200 | { 201 | "cell_type": "code", 202 | "execution_count": null, 203 | "id": "2bd2e909-f54c-4da7-b943-2e0f23a43a29", 204 | "metadata": {}, 205 | "outputs": [], 206 | "source": [ 207 | "#Position trajectories match\n", 208 | "plot(ytraj1)\n", 209 | "plot(ytraj2)\n", 210 | "ytraj1-ytraj2" 211 | ] 212 | }, 213 | { 214 | "cell_type": "code", 215 | "execution_count": null, 216 | "id": "966c23ef-d3b6-42e2-9210-4851d09e8299", 217 | "metadata": {}, 218 | "outputs": [], 219 | "source": [ 220 | "#Generate a random trajectory by \"de-noising\"\n", 221 | "z̃ = randn(T)\n", 222 | "plot(z̃)" 223 | ] 224 | }, 225 | { 226 | "cell_type": "code", 227 | "execution_count": null, 228 | "id": "ffcb448d-4852-4a3e-99df-8b70601cde45", 229 | "metadata": {}, 230 | "outputs": [], 231 | "source": [ 232 | "w = (H'*H)\\(H'*z̃)\n", 233 | "z = H*w\n", 234 | "plot(z)" 235 | ] 236 | }, 237 | { 238 | "cell_type": "code", 239 | "execution_count": null, 240 | "id": "b5e458d0-1247-401b-b229-cc142f43035f", 241 | "metadata": {}, 242 | "outputs": [], 243 | "source": [ 244 | "#Now let's do a closed-loop sim with an LQR controller\n", 245 | "\n", 246 | "using ControlSystems\n", 247 | "Q = Array(1.0*I(2))\n", 248 | "R = Array(0.1*I(1))\n", 249 | "\n", 250 | "AB = exp(h*[0 1 0; -k -c 1; 0 0 0])\n", 251 | "A = AB[1:2,1:2]\n", 252 | "B = AB[1:2,3]\n", 253 | "\n", 254 | "K = dlqr(A,B,Q,R)" 255 | ] 256 | }, 257 | { 258 | "cell_type": "code", 259 | "execution_count": null, 260 | "id": "02c73106-12d8-448b-8b83-0a260eddf020", 261 | "metadata": {}, 262 | "outputs": [], 263 | "source": [ 264 | "x0 = [1.0; 0]\n", 265 | "xtraj = zeros(2,T);\n", 266 | "utraj = zeros(T-1);\n", 267 | "xtraj[:,1] .= x0\n", 268 | "for k = 1:(T-1)\n", 269 | " utraj[k] = -(K*xtraj[:,k])[1]\n", 270 | " xtraj[:,k+1] .= (A-B*K)*xtraj[:,k];\n", 271 | "end\n", 272 | "\n", 273 | "plot(xtraj[1,:])\n", 274 | "plot(xtraj[2,:])" 275 | ] 276 | }, 277 | { 278 | "cell_type": "code", 279 | "execution_count": null, 280 | "id": "b43c5240-b5bc-4a24-83ce-986d7f964618", 281 | "metadata": {}, 282 | "outputs": [], 283 | "source": [ 284 | "#Generate some random closed-loop trajectories\n", 285 | "N = 10\n", 286 | "H = zeros(3*T,N)\n", 287 | "for ℓ = 1:N\n", 288 | " xtraj = zeros(2,T)\n", 289 | " xtraj[:,1] .= randn(2)\n", 290 | " utraj = zeros(T);\n", 291 | " utraj[:,1] .= -(K*xtraj[:,1])[1]\n", 292 | " for k = 1:(T-1)\n", 293 | " xtraj[:,k+1] .= A*xtraj[:,k] + B*utraj[k]\n", 294 | " utraj[k+1] = -(K*xtraj[:,k+1])[1]\n", 295 | " end\n", 296 | " H[:,ℓ] .= ([xtraj; utraj'])[:]\n", 297 | "end" 298 | ] 299 | }, 300 | { 301 | "cell_type": "code", 302 | "execution_count": null, 303 | "id": "f50b2b6b-845a-41d7-820a-65b94c5bcd00", 304 | "metadata": {}, 305 | "outputs": [], 306 | "source": [ 307 | "rank(H) #rank of data matrix is still the same" 308 | ] 309 | }, 310 | { 311 | "cell_type": "code", 312 | "execution_count": null, 313 | "id": "4e418ab4-3c93-4869-933c-75d2c9cc0a87", 314 | "metadata": {}, 315 | "outputs": [], 316 | "source": [ 317 | "#Predict the closed-loop state-control trajectory from the first state using the data matrix\n", 318 | "E = [I zeros(2,3*T-2)]\n", 319 | "w = (H'*E'*E*H)\\(H'*E'*x0)\n", 320 | "ztraj = H*w" 321 | ] 322 | }, 323 | { 324 | "cell_type": "code", 325 | "execution_count": null, 326 | "id": "5efa5d88-62ce-4269-9e4e-4ed9021720a7", 327 | "metadata": {}, 328 | "outputs": [], 329 | "source": [ 330 | "#Pick off the first control\n", 331 | "U = [0 0 1 zeros(1,3*T-3)]\n", 332 | "u = U*ztraj" 333 | ] 334 | }, 335 | { 336 | "cell_type": "code", 337 | "execution_count": null, 338 | "id": "411d1c87-2b2f-45f7-8161-16699cc95ef4", 339 | "metadata": {}, 340 | "outputs": [], 341 | "source": [ 342 | "#matches the LQR controller\n", 343 | "-K*x0" 344 | ] 345 | }, 346 | { 347 | "cell_type": "code", 348 | "execution_count": null, 349 | "id": "530c3993-72af-445b-a621-d5e353c0b566", 350 | "metadata": {}, 351 | "outputs": [], 352 | "source": [ 353 | "#We can even recover the LQR gain from the data matrix\n", 354 | "-U*H*((H'*E'*E*H)\\(H'*E'))" 355 | ] 356 | }, 357 | { 358 | "cell_type": "code", 359 | "execution_count": null, 360 | "id": "a4c9aed1-38ae-4453-8606-4474155cab45", 361 | "metadata": {}, 362 | "outputs": [], 363 | "source": [ 364 | "K" 365 | ] 366 | }, 367 | { 368 | "cell_type": "code", 369 | "execution_count": null, 370 | "id": "95c74ddf-091c-48b0-baf1-18ee302b3276", 371 | "metadata": {}, 372 | "outputs": [], 373 | "source": [ 374 | "#What if we drive the system with white noise instead of using LQR?\n", 375 | "N = 100\n", 376 | "H = zeros(3*T-1,N)\n", 377 | "for ℓ = 1:N\n", 378 | " xtraj = zeros(2,T)\n", 379 | " xtraj[:,1] .= randn(2)\n", 380 | " utraj = zeros(T-1);\n", 381 | " for k = 1:(T-1)\n", 382 | " utraj[k] = 0.1*randn();\n", 383 | " xtraj[:,k+1] .= A*xtraj[:,k] + B*utraj[k]\n", 384 | " end\n", 385 | " H[:,ℓ] .= [([xtraj[:,1:end-1]; utraj'])[:]; xtraj[:,end]]\n", 386 | "end" 387 | ] 388 | }, 389 | { 390 | "cell_type": "code", 391 | "execution_count": null, 392 | "id": "546b01f8-7bd5-44c2-96d8-c1535145145d", 393 | "metadata": {}, 394 | "outputs": [], 395 | "source": [ 396 | "rank(H)" 397 | ] 398 | }, 399 | { 400 | "cell_type": "code", 401 | "execution_count": null, 402 | "id": "41f4eb2a-4eff-4e9f-bb35-450d25305574", 403 | "metadata": {}, 404 | "outputs": [], 405 | "source": [ 406 | "#Do MPC with the data matrix\n", 407 | "Nz = 3*T-1\n", 408 | "\n", 409 | "Q = Diagonal([kron(ones(T-1),[1; 1; 1.0]); 1; 1])\n", 410 | "α = 1e-4;\n", 411 | "Y = [I zeros(2,147)]\n", 412 | "\n", 413 | "#KKT matrix\n", 414 | "F = factorize( [Q zeros(Nz,N) -I Y';\n", 415 | " zeros(N,Nz) α*I H' zeros(N,2);\n", 416 | " -I H zeros(Nz,Nz) zeros(Nz,2);\n", 417 | " Y zeros(2,N+Nz+2)]);" 418 | ] 419 | }, 420 | { 421 | "cell_type": "code", 422 | "execution_count": null, 423 | "id": "f9fc0272-785b-4fa8-8f59-c711bf1a69da", 424 | "metadata": {}, 425 | "outputs": [], 426 | "source": [ 427 | "#Closed-loop sim matches LQR\n", 428 | "x0 = [1.0; 0]\n", 429 | "xtraj = zeros(2,T);\n", 430 | "utraj = zeros(T-1);\n", 431 | "xtraj[:,1] .= x0\n", 432 | "for k = 1:(T-1)\n", 433 | " #solve QP\n", 434 | " s = F\\[zeros(149+100+149); xtraj[:,k]];\n", 435 | "\n", 436 | " utraj[k] = s[3]\n", 437 | " xtraj[:,k+1] .= A*xtraj[:,k] + B*utraj[k];\n", 438 | "end" 439 | ] 440 | }, 441 | { 442 | "cell_type": "code", 443 | "execution_count": null, 444 | "id": "baab5b85-4a39-4924-9d66-0456128179de", 445 | "metadata": {}, 446 | "outputs": [], 447 | "source": [ 448 | "plot(xtraj[1,:])\n", 449 | "plot(xtraj[2,:])" 450 | ] 451 | }, 452 | { 453 | "cell_type": "code", 454 | "execution_count": null, 455 | "id": "8e600cd7-3245-469f-b7a8-6c9b34ca6bcc", 456 | "metadata": {}, 457 | "outputs": [], 458 | "source": [] 459 | } 460 | ], 461 | "metadata": { 462 | "kernelspec": { 463 | "display_name": "Julia 1.10.9", 464 | "language": "julia", 465 | "name": "julia-1.10" 466 | }, 467 | "language_info": { 468 | "file_extension": ".jl", 469 | "mimetype": "application/julia", 470 | "name": "julia", 471 | "version": "1.10.9" 472 | } 473 | }, 474 | "nbformat": 4, 475 | "nbformat_minor": 5 476 | } 477 | -------------------------------------------------------------------------------- /Lecture 24/quadrotor-clone.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "d7acada4-99f0-4c03-9027-bd7afe7827fb", 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": "f3737630-e86b-46fb-8c46-08761b4f35d9", 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "using LinearAlgebra\n", 21 | "using ControlSystems\n", 22 | "using ForwardDiff\n", 23 | "using PyPlot" 24 | ] 25 | }, 26 | { 27 | "cell_type": "code", 28 | "execution_count": null, 29 | "id": "799fc21d-b3ba-4591-a986-2ade8391dc09", 30 | "metadata": {}, 31 | "outputs": [], 32 | "source": [ 33 | "#Model parameters\n", 34 | "g = 9.81 #m/s^2\n", 35 | "m = 1.0 #kg \n", 36 | "ℓ = 0.3 #meters\n", 37 | "J = 0.2*m*ℓ*ℓ\n", 38 | "\n", 39 | "#Thrust limits\n", 40 | "umin = [0.2*m*g; 0.2*m*g]\n", 41 | "umax = [0.6*m*g; 0.6*m*g]\n", 42 | "\n", 43 | "h = 0.05; #time step (20 Hz)" 44 | ] 45 | }, 46 | { 47 | "cell_type": "code", 48 | "execution_count": null, 49 | "id": "0f1ba9c8-50fd-4fa0-b909-01b8cdbf8037", 50 | "metadata": {}, 51 | "outputs": [], 52 | "source": [ 53 | "#Planar Quadrotor Dynamics\n", 54 | "function quad_dynamics(x,u)\n", 55 | " θ = x[3]\n", 56 | " \n", 57 | " ẍ = (1/m)*(u[1] + u[2])*sin(θ)\n", 58 | " ÿ = (1/m)*(u[1] + u[2])*cos(θ) - g\n", 59 | " θ̈ = (1/J)*(ℓ/2)*(u[2] - u[1])\n", 60 | " \n", 61 | " return [x[4:6]; ẍ; ÿ; θ̈]\n", 62 | "end\n", 63 | "\n", 64 | "function quad_dynamics_rk4(x,u)\n", 65 | " #RK4 integration with zero-order hold on u\n", 66 | " f1 = quad_dynamics(x, u)\n", 67 | " f2 = quad_dynamics(x + 0.5*h*f1, u)\n", 68 | " f3 = quad_dynamics(x + 0.5*h*f2, u)\n", 69 | " f4 = quad_dynamics(x + h*f3, u)\n", 70 | " return x + (h/6.0)*(f1 + 2*f2 + 2*f3 + f4)\n", 71 | "end\n", 72 | "\n", 73 | "#Linearized dynamics for hovering\n", 74 | "Nx = 6 # number of state\n", 75 | "Ny = 3 # number of observations\n", 76 | "Nu = 2 # number of controls\n", 77 | "x_hover = zeros(6)\n", 78 | "u_hover = [0.5*m*g; 0.5*m*g]\n", 79 | "A = ForwardDiff.jacobian(x->quad_dynamics_rk4(x,u_hover),x_hover);\n", 80 | "B = ForwardDiff.jacobian(u->quad_dynamics_rk4(x_hover,u),u_hover);\n", 81 | "C = [I zeros(3,3)];" 82 | ] 83 | }, 84 | { 85 | "cell_type": "code", 86 | "execution_count": null, 87 | "id": "982b64fc-8034-4297-857a-81d3b0891fac", 88 | "metadata": {}, 89 | "outputs": [], 90 | "source": [ 91 | "#Set up visualization\n", 92 | "using MeshCat\n", 93 | "using RobotZoo: Quadrotor, PlanarQuadrotor\n", 94 | "using CoordinateTransformations, Rotations, Colors, StaticArrays, RobotDynamics\n", 95 | "\n", 96 | "function set_mesh!(vis, model::L;\n", 97 | " scaling=1.0, color=colorant\"black\"\n", 98 | " ) where {L <: Union{Quadrotor, PlanarQuadrotor}} \n", 99 | " # urdf_folder = joinpath(@__DIR__, \"..\", \"data\", \"meshes\")\n", 100 | " urdf_folder = @__DIR__\n", 101 | " # if scaling != 1.0\n", 102 | " # quad_scaling = 0.085 * scaling\n", 103 | " obj = joinpath(urdf_folder, \"quadrotor_scaled.obj\")\n", 104 | " if scaling != 1.0\n", 105 | " error(\"Scaling not implemented after switching to MeshCat 0.12\")\n", 106 | " end\n", 107 | " robot_obj = MeshFileGeometry(obj)\n", 108 | " mat = MeshPhongMaterial(color=color)\n", 109 | " setobject!(vis[\"robot\"][\"geom\"], robot_obj, mat)\n", 110 | " if hasfield(L, :ned)\n", 111 | " model.ned && settransform!(vis[\"robot\"][\"geom\"], LinearMap(RotX(pi)))\n", 112 | " end\n", 113 | "end\n", 114 | "\n", 115 | "function visualize!(vis, model::PlanarQuadrotor, x::StaticVector)\n", 116 | " py,pz = x[1], x[2]\n", 117 | " θ = x[3]\n", 118 | " settransform!(vis[\"robot\"], compose(Translation(0,py,pz), LinearMap(RotX(-θ))))\n", 119 | "end\n", 120 | "\n", 121 | "function visualize!(vis, model, tf::Real, X)\n", 122 | " fps = Int(round((length(X)-1)/tf))\n", 123 | " anim = MeshCat.Animation(vis; fps)\n", 124 | " for (k,x) in enumerate(X)\n", 125 | " atframe(anim, k) do\n", 126 | " x = X[k]\n", 127 | " visualize!(vis, model, SVector{6}(x)) \n", 128 | " end\n", 129 | " end\n", 130 | " setanimation!(vis, anim)\n", 131 | "end" 132 | ] 133 | }, 134 | { 135 | "cell_type": "code", 136 | "execution_count": null, 137 | "id": "d8529227-2642-40d9-8a8d-a6d0ffb8f532", 138 | "metadata": {}, 139 | "outputs": [], 140 | "source": [ 141 | "#LQR Hover Controller\n", 142 | "Q = Array(1.0*I(Nx));\n", 143 | "R = Array(0.1*I(Nu));\n", 144 | "K = dlqr(A,B,Q,R)\n", 145 | "\n", 146 | "xref = [0.0; 1.0; 0; 0; 0; 0]\n", 147 | "\n", 148 | "function lqr_controller(x)\n", 149 | " return u_hover - K*(x-xref)\n", 150 | "end" 151 | ] 152 | }, 153 | { 154 | "cell_type": "code", 155 | "execution_count": null, 156 | "id": "7c760580-d08f-49f2-9c7b-d49a98990883", 157 | "metadata": {}, 158 | "outputs": [], 159 | "source": [ 160 | "Tfinal = 5.0 # final time\n", 161 | "Nt = Int(Tfinal/h)+1 # number of time steps\n", 162 | "thist = Array(range(0,h*(Nt-1), step=h));\n", 163 | "\n", 164 | "function closed_loop(x0,N)\n", 165 | " xhist = zeros(Nx,N)\n", 166 | " uhist = zeros(Nu,N-1)\n", 167 | " xhist[:,1] .= x0\n", 168 | " for k = 1:(N-1)\n", 169 | " uhist[:,k] .= lqr_controller(xhist[:,k])\n", 170 | " xhist[:,k+1] .= xref + A*(xhist[:,k]-xref) + B*(uhist[:,k]-u_hover)\n", 171 | " end\n", 172 | " return xhist, uhist\n", 173 | "end" 174 | ] 175 | }, 176 | { 177 | "cell_type": "code", 178 | "execution_count": null, 179 | "id": "77c074a9-c2d8-437e-b50c-69051b6af320", 180 | "metadata": {}, 181 | "outputs": [], 182 | "source": [ 183 | "#Generate a bunch of trajectories from random initial conditions\n", 184 | "N = 20\n", 185 | "H = zeros((Ny+Nu)*(Nt-1),N)\n", 186 | "for ℓ = 1:N\n", 187 | " x0 = xref + 0.2*randn(Nx)\n", 188 | " xtraj, utraj = closed_loop(x0, Nt);\n", 189 | " ytraj = C*xtraj[:,1:end-1];\n", 190 | " H[:,ℓ] = ([ytraj; utraj])[:]\n", 191 | "end" 192 | ] 193 | }, 194 | { 195 | "cell_type": "code", 196 | "execution_count": null, 197 | "id": "908b5835-ad10-4172-828b-41c05886b506", 198 | "metadata": {}, 199 | "outputs": [], 200 | "source": [ 201 | "#Cloned controller\n", 202 | "m = 2; #observation history\n", 203 | "function cloned_controller(yhist)\n", 204 | " Y = [I zeros(m*Ny + (m-1)*Nu,(Ny*(Nt-m-1) + Nu*(Nt-m)))]\n", 205 | " U = [zeros(Nu,Ny*m + Nu*(m-1)) I zeros(Nu,(Ny+Nu)*(Nt-m-1))]\n", 206 | " w = (H'*Y'*Y*H)\\(H'*Y'*yhist[:])\n", 207 | " u = U*H*w\n", 208 | " return u\n", 209 | "end" 210 | ] 211 | }, 212 | { 213 | "cell_type": "code", 214 | "execution_count": null, 215 | "id": "52b3f21a-8d24-4d88-8a65-961af4366810", 216 | "metadata": {}, 217 | "outputs": [], 218 | "source": [ 219 | "function closed_loop2(x0,N)\n", 220 | " xhist = zeros(Nx,N)\n", 221 | " uhist = zeros(Nu,N-1)\n", 222 | " xhist[:,1] .= x0\n", 223 | " for k = 1:(m-1)\n", 224 | " uhist[:,k] .= lqr_controller(xhist[:,k]);\n", 225 | " xhist[:,k+1] .= xref + A*(xhist[:,k]-xref) + B*(uhist[:,k]-u_hover)\n", 226 | " end\n", 227 | " for k = m:(N-1)\n", 228 | " yhist = [C*xhist[:,k-1]; uhist[:,k-1]; C*xhist[:,k]]\n", 229 | " uhist[:,k] .= cloned_controller(yhist)\n", 230 | " xhist[:,k+1] .= xref + A*(xhist[:,k]-xref) + B*(uhist[:,k]-u_hover)\n", 231 | " end\n", 232 | " return xhist, uhist\n", 233 | "end" 234 | ] 235 | }, 236 | { 237 | "cell_type": "code", 238 | "execution_count": null, 239 | "id": "5cd1b5f4-7e9a-42ed-8928-a194dc461f39", 240 | "metadata": {}, 241 | "outputs": [], 242 | "source": [ 243 | "vis = Visualizer()\n", 244 | "model = PlanarQuadrotor()\n", 245 | "set_mesh!(vis, model)\n", 246 | "render(vis)" 247 | ] 248 | }, 249 | { 250 | "cell_type": "code", 251 | "execution_count": null, 252 | "id": "5bb80d87-fa52-4bf5-9c36-db79be1a719a", 253 | "metadata": {}, 254 | "outputs": [], 255 | "source": [ 256 | "#Rollout from random initial conditions\n", 257 | "x0 = xref + 0.5*randn(Nx)\n", 258 | "xhist1, uhist1 = closed_loop(x0, Nt);\n", 259 | "xhist2, uhist2 = closed_loop2(x0, Nt);\n", 260 | "xhist1-xhist2" 261 | ] 262 | }, 263 | { 264 | "cell_type": "code", 265 | "execution_count": null, 266 | "id": "f6f98e14-5dfa-48b6-836f-1ade31fb4d8f", 267 | "metadata": {}, 268 | "outputs": [], 269 | "source": [ 270 | "X2 = [SVector{6}(x) for x in eachcol(xhist2)];\n", 271 | "visualize!(vis, model, thist[end], X2)" 272 | ] 273 | }, 274 | { 275 | "cell_type": "code", 276 | "execution_count": null, 277 | "id": "f9af17dd-ede3-4f39-8827-233a1e24b6d7", 278 | "metadata": {}, 279 | "outputs": [], 280 | "source": [] 281 | } 282 | ], 283 | "metadata": { 284 | "kernelspec": { 285 | "display_name": "Julia 1.10.9", 286 | "language": "julia", 287 | "name": "julia-1.10" 288 | }, 289 | "language_info": { 290 | "file_extension": ".jl", 291 | "mimetype": "application/julia", 292 | "name": "julia", 293 | "version": "1.10.9" 294 | } 295 | }, 296 | "nbformat": 4, 297 | "nbformat_minor": 5 298 | } 299 | -------------------------------------------------------------------------------- /Lecture 3/Lecture 3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 3/Lecture 3.pdf -------------------------------------------------------------------------------- /Lecture 3/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | julia_version = "1.10.7" 4 | manifest_format = "2.0" 5 | project_hash = "da39a3ee5e6b4b0d3255bfef95601890afd80709" 6 | 7 | [deps] 8 | -------------------------------------------------------------------------------- /Lecture 3/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | -------------------------------------------------------------------------------- /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 | "∇2f(0.0)" 115 | ] 116 | }, 117 | { 118 | "cell_type": "code", 119 | "execution_count": null, 120 | "metadata": {}, 121 | "outputs": [], 122 | "source": [ 123 | "function regularized_newton_step(x0)\n", 124 | " β = 1.0\n", 125 | " H = ∇2f(x0)\n", 126 | " while !isposdef(H)\n", 127 | " H = H + β*I\n", 128 | " end\n", 129 | " xn = x0 - H\\∇f(x0)\n", 130 | "end" 131 | ] 132 | }, 133 | { 134 | "cell_type": "code", 135 | "execution_count": null, 136 | "metadata": {}, 137 | "outputs": [], 138 | "source": [ 139 | "xguess = 0.0\n", 140 | "plot(x, f(x))\n", 141 | "plot(xguess, f(xguess), \"rx\")" 142 | ] 143 | }, 144 | { 145 | "cell_type": "code", 146 | "execution_count": null, 147 | "metadata": {}, 148 | "outputs": [], 149 | "source": [ 150 | "xnew = regularized_newton_step(xguess[end])\n", 151 | "xguess = [xguess xnew]\n", 152 | "plot(x, f(x))\n", 153 | "plot(xguess, f(xguess), \"rx\")" 154 | ] 155 | }, 156 | { 157 | "cell_type": "code", 158 | "execution_count": null, 159 | "metadata": {}, 160 | "outputs": [], 161 | "source": [] 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.10.7", 174 | "language": "julia", 175 | "name": "julia-1.10" 176 | }, 177 | "language_info": { 178 | "file_extension": ".jl", 179 | "mimetype": "application/julia", 180 | "name": "julia", 181 | "version": "1.10.7" 182 | } 183 | }, 184 | "nbformat": 4, 185 | "nbformat_minor": 4 186 | } 187 | -------------------------------------------------------------------------------- /Lecture 3/root-finding.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using ForwardDiff\n", 20 | "using PyPlot" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "function pendulum_dynamics(x)\n", 30 | " l = 1.0\n", 31 | " g = 9.81\n", 32 | " \n", 33 | " θ = x[1]\n", 34 | " θ̇ = x[2]\n", 35 | " \n", 36 | " θ̈ = -(g/l)*sin(θ)\n", 37 | " \n", 38 | " return [θ̇; θ̈]\n", 39 | "end" 40 | ] 41 | }, 42 | { 43 | "cell_type": "code", 44 | "execution_count": null, 45 | "metadata": {}, 46 | "outputs": [], 47 | "source": [ 48 | "function backward_euler_step_fixed_point(fun, x0, h)\n", 49 | " xn = x0\n", 50 | " e = [norm(x0 + h.*fun(xn) - xn)]\n", 51 | " while e[end] > 1e-8\n", 52 | " xn = x0 + h.*fun(xn)\n", 53 | " e = [e; norm(x0 + h.*fun(xn) - xn)]\n", 54 | " end\n", 55 | " \n", 56 | " return xn, e\n", 57 | "end" 58 | ] 59 | }, 60 | { 61 | "cell_type": "code", 62 | "execution_count": null, 63 | "metadata": {}, 64 | "outputs": [], 65 | "source": [ 66 | "function backward_euler_step_newton(fun, x0, h)\n", 67 | " xn = x0\n", 68 | " r = x0 + h.*fun(xn) - xn\n", 69 | " e = [norm(r)]\n", 70 | " while e[end] > 1e-8\n", 71 | " ∂r = ForwardDiff.jacobian(x -> x0 + h.*fun(x) - x, xn)\n", 72 | " xn = xn - ∂r\\r\n", 73 | " r = x0 + h.*fun(xn) - xn\n", 74 | " e = [e; norm(r)]\n", 75 | " end\n", 76 | " \n", 77 | " return xn, e\n", 78 | "end" 79 | ] 80 | }, 81 | { 82 | "cell_type": "code", 83 | "execution_count": null, 84 | "metadata": {}, 85 | "outputs": [], 86 | "source": [ 87 | "function backward_euler_fixed_point(fun, x0, Tf, h)\n", 88 | " t = Array(range(0,Tf,step=h))\n", 89 | " \n", 90 | " x_hist = zeros(length(x0),length(t))\n", 91 | " x_hist[:,1] .= x0\n", 92 | " \n", 93 | " for k = 1:(length(t)-1)\n", 94 | " x_hist[:,k+1], e = backward_euler_step_fixed_point(fun, x_hist[:,k], h)\n", 95 | " end\n", 96 | " \n", 97 | " return x_hist, t\n", 98 | "end" 99 | ] 100 | }, 101 | { 102 | "cell_type": "code", 103 | "execution_count": null, 104 | "metadata": {}, 105 | "outputs": [], 106 | "source": [ 107 | "function backward_euler_newton(fun, x0, Tf, h)\n", 108 | " t = Array(range(0,Tf,step=h))\n", 109 | " \n", 110 | " x_hist = zeros(length(x0),length(t))\n", 111 | " x_hist[:,1] .= x0\n", 112 | " \n", 113 | " for k = 1:(length(t)-1)\n", 114 | " x_hist[:,k+1], e = backward_euler_step_newton(fun, x_hist[:,k], h)\n", 115 | " end\n", 116 | " \n", 117 | " return x_hist, t\n", 118 | "end" 119 | ] 120 | }, 121 | { 122 | "cell_type": "code", 123 | "execution_count": null, 124 | "metadata": {}, 125 | "outputs": [], 126 | "source": [ 127 | "x0 = [.1; 0]\n", 128 | "x_hist1, t_hist1 = backward_euler_fixed_point(pendulum_dynamics, x0, 10, 0.01)\n", 129 | "x_hist2, t_hist2 = backward_euler_newton(pendulum_dynamics, x0, 10, 0.01)\n", 130 | "plot(t_hist1, x_hist1[1,:])\n", 131 | "plot(t_hist2, x_hist2[1,:])" 132 | ] 133 | }, 134 | { 135 | "cell_type": "code", 136 | "execution_count": null, 137 | "metadata": {}, 138 | "outputs": [], 139 | "source": [ 140 | "max(abs.(x_hist1-x_hist2)...)" 141 | ] 142 | }, 143 | { 144 | "cell_type": "code", 145 | "execution_count": null, 146 | "metadata": {}, 147 | "outputs": [], 148 | "source": [ 149 | "xn, e1 = backward_euler_step_fixed_point(pendulum_dynamics, x0, 0.1)\n", 150 | "e1" 151 | ] 152 | }, 153 | { 154 | "cell_type": "code", 155 | "execution_count": null, 156 | "metadata": {}, 157 | "outputs": [], 158 | "source": [ 159 | "xn, e2 = backward_euler_step_newton(pendulum_dynamics, x0, 0.1)\n", 160 | "e2" 161 | ] 162 | }, 163 | { 164 | "cell_type": "code", 165 | "execution_count": null, 166 | "metadata": {}, 167 | "outputs": [], 168 | "source": [ 169 | "semilogy(e1)\n", 170 | "semilogy(e2)" 171 | ] 172 | }, 173 | { 174 | "cell_type": "code", 175 | "execution_count": null, 176 | "metadata": {}, 177 | "outputs": [], 178 | "source": [] 179 | }, 180 | { 181 | "cell_type": "code", 182 | "execution_count": null, 183 | "metadata": {}, 184 | "outputs": [], 185 | "source": [] 186 | } 187 | ], 188 | "metadata": { 189 | "kernelspec": { 190 | "display_name": "Julia 1.10.7", 191 | "language": "julia", 192 | "name": "julia-1.10" 193 | }, 194 | "language_info": { 195 | "file_extension": ".jl", 196 | "mimetype": "application/julia", 197 | "name": "julia", 198 | "version": "1.10.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/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 4/Lecture 4.pdf -------------------------------------------------------------------------------- /Lecture 4/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | julia_version = "1.10.7" 4 | manifest_format = "2.0" 5 | project_hash = "da39a3ee5e6b4b0d3255bfef95601890afd80709" 6 | 7 | [deps] 8 | -------------------------------------------------------------------------------- /Lecture 4/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | -------------------------------------------------------------------------------- /Lecture 4/equality-constraints.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using ForwardDiff\n", 20 | "using PyPlot" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "Q = Diagonal([0.5; 1])\n", 30 | "function f(x)\n", 31 | " return 0.5*(x-[1; 0])'*Q*(x-[1; 0])\n", 32 | "end\n", 33 | "function ∇f(x)\n", 34 | " return Q*(x-[1; 0])\n", 35 | "end\n", 36 | "function ∇2f(x)\n", 37 | " return Q\n", 38 | "end" 39 | ] 40 | }, 41 | { 42 | "cell_type": "code", 43 | "execution_count": null, 44 | "metadata": {}, 45 | "outputs": [], 46 | "source": [ 47 | "function c(x)\n", 48 | " return x[1]^2 + 2*x[1] - x[2]\n", 49 | "end\n", 50 | "function ∂c(x)\n", 51 | " return [2*x[1]+2 -1]\n", 52 | "end" 53 | ] 54 | }, 55 | { 56 | "cell_type": "code", 57 | "execution_count": null, 58 | "metadata": {}, 59 | "outputs": [], 60 | "source": [ 61 | "function plot_landscape()\n", 62 | " Nsamp = 20\n", 63 | " Xsamp = kron(ones(Nsamp),LinRange(-4,4,Nsamp)')\n", 64 | " Ysamp = kron(ones(Nsamp)',LinRange(-4,4,Nsamp))\n", 65 | " Zsamp = zeros(Nsamp,Nsamp)\n", 66 | " for j = 1:Nsamp\n", 67 | " for k = 1:Nsamp\n", 68 | " Zsamp[j,k] = f([Xsamp[j,k]; Ysamp[j,k]])\n", 69 | " end\n", 70 | " end\n", 71 | " contour(Xsamp,Ysamp,Zsamp)\n", 72 | "\n", 73 | " xc = LinRange(-3.2,1.2,Nsamp)\n", 74 | " plot(xc,xc.^2+2.0.*xc,\"y\")\n", 75 | "end\n", 76 | "\n", 77 | "plot_landscape()" 78 | ] 79 | }, 80 | { 81 | "cell_type": "code", 82 | "execution_count": null, 83 | "metadata": {}, 84 | "outputs": [], 85 | "source": [ 86 | "function newton_step(x0,λ0)\n", 87 | " H = ∇2f(x0) + ForwardDiff.jacobian(x -> ∂c(x)'*λ0, x0)\n", 88 | " C = ∂c(x0)\n", 89 | " Δz = [H C'; C 0]\\[-∇f(x0)-C'*λ0; -c(x0)]\n", 90 | " Δx = Δz[1:2]\n", 91 | " Δλ = Δz[3]\n", 92 | " return x0+Δx, λ0+Δλ\n", 93 | "end" 94 | ] 95 | }, 96 | { 97 | "cell_type": "code", 98 | "execution_count": null, 99 | "metadata": {}, 100 | "outputs": [], 101 | "source": [ 102 | "xguess = [-1; -1]\n", 103 | "λguess = [0.0]\n", 104 | "plot_landscape()\n", 105 | "plot(xguess[1], xguess[2], \"rx\")" 106 | ] 107 | }, 108 | { 109 | "cell_type": "code", 110 | "execution_count": null, 111 | "metadata": {}, 112 | "outputs": [], 113 | "source": [ 114 | "xnew, λnew = newton_step(xguess[:,end],λguess[end])\n", 115 | "xguess = [xguess xnew]\n", 116 | "λguess = [λguess λnew]\n", 117 | "plot_landscape()\n", 118 | "plot(xguess[1,:], xguess[2,:], \"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 | "∇2f(xguess[:,end])" 137 | ] 138 | }, 139 | { 140 | "cell_type": "code", 141 | "execution_count": null, 142 | "metadata": {}, 143 | "outputs": [], 144 | "source": [ 145 | "function gauss_newton_step(x0,λ0)\n", 146 | " H = ∇2f(x0)\n", 147 | " C = ∂c(x0)\n", 148 | " Δz = [H C'; C 0]\\[-∇f(x0)-C'*λ0; -c(x0)]\n", 149 | " Δx = Δz[1:2]\n", 150 | " Δλ = Δz[3]\n", 151 | " return x0+Δx, λ0+Δλ\n", 152 | "end" 153 | ] 154 | }, 155 | { 156 | "cell_type": "code", 157 | "execution_count": null, 158 | "metadata": {}, 159 | "outputs": [], 160 | "source": [ 161 | "xguess = [-3; 2]\n", 162 | "λguess = [0.0]\n", 163 | "plot_landscape()\n", 164 | "plot(xguess[1], xguess[2], \"rx\")" 165 | ] 166 | }, 167 | { 168 | "cell_type": "code", 169 | "execution_count": null, 170 | "metadata": {}, 171 | "outputs": [], 172 | "source": [ 173 | "xnew, λnew = gauss_newton_step(xguess[:,end],λguess[end])\n", 174 | "xguess = [xguess xnew]\n", 175 | "λguess = [λguess λnew]\n", 176 | "plot_landscape()\n", 177 | "plot(xguess[1,:], xguess[2,:], \"rx\")" 178 | ] 179 | }, 180 | { 181 | "cell_type": "code", 182 | "execution_count": null, 183 | "metadata": {}, 184 | "outputs": [], 185 | "source": [] 186 | }, 187 | { 188 | "cell_type": "code", 189 | "execution_count": null, 190 | "metadata": {}, 191 | "outputs": [], 192 | "source": [] 193 | } 194 | ], 195 | "metadata": { 196 | "kernelspec": { 197 | "display_name": "Julia 1.10.7", 198 | "language": "julia", 199 | "name": "julia-1.10" 200 | }, 201 | "language_info": { 202 | "file_extension": ".jl", 203 | "mimetype": "application/julia", 204 | "name": "julia", 205 | "version": "1.10.7" 206 | } 207 | }, 208 | "nbformat": 4, 209 | "nbformat_minor": 4 210 | } 211 | -------------------------------------------------------------------------------- /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 | " print(α)\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.10.7", 229 | "language": "julia", 230 | "name": "julia-1.10" 231 | }, 232 | "language_info": { 233 | "file_extension": ".jl", 234 | "mimetype": "application/julia", 235 | "name": "julia", 236 | "version": "1.10.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/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 5/Lecture 5.pdf -------------------------------------------------------------------------------- /Lecture 5/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | julia_version = "1.10.7" 4 | manifest_format = "2.0" 5 | project_hash = "da39a3ee5e6b4b0d3255bfef95601890afd80709" 6 | 7 | [deps] 8 | -------------------------------------------------------------------------------- /Lecture 5/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | -------------------------------------------------------------------------------- /Lecture 5/interior-point.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "id": "012f9b4d-9754-467b-9b9c-7367f77ba292", 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": "d129e500-1db0-4f09-ba14-1c60d953792a", 17 | "metadata": {}, 18 | "outputs": [], 19 | "source": [ 20 | "using LinearAlgebra\n", 21 | "using ForwardDiff\n", 22 | "using PyPlot" 23 | ] 24 | }, 25 | { 26 | "cell_type": "code", 27 | "execution_count": null, 28 | "id": "65202fb8-a805-4033-93c0-6a4cdf7810c9", 29 | "metadata": {}, 30 | "outputs": [], 31 | "source": [ 32 | "Q = Diagonal([0.5; 1])\n", 33 | "function f(x)\n", 34 | " return 0.5*(x-[1; 0])'*Q*(x-[1; 0])\n", 35 | "end\n", 36 | "function ∇f(x)\n", 37 | " return Q*(x-[1; 0])\n", 38 | "end\n", 39 | "function ∇2f(x)\n", 40 | " return Q\n", 41 | "end" 42 | ] 43 | }, 44 | { 45 | "cell_type": "code", 46 | "execution_count": null, 47 | "id": "583fbdc4-375b-4013-8064-f00842c19240", 48 | "metadata": {}, 49 | "outputs": [], 50 | "source": [ 51 | "A = [-1.0 1.0]\n", 52 | "b = 1.0\n", 53 | "function c(x)\n", 54 | " return dot(A,x) - b\n", 55 | "end\n", 56 | "function ∂c(x)\n", 57 | " return A\n", 58 | "end" 59 | ] 60 | }, 61 | { 62 | "cell_type": "code", 63 | "execution_count": null, 64 | "id": "732a988e-de84-47c6-a24e-c262f45f2896", 65 | "metadata": {}, 66 | "outputs": [], 67 | "source": [ 68 | "function plot_landscape()\n", 69 | " Nsamp = 20\n", 70 | " Xsamp = kron(ones(Nsamp),LinRange(-4,4,Nsamp)')\n", 71 | " Ysamp = kron(ones(Nsamp)',LinRange(-4,4,Nsamp))\n", 72 | " Zsamp = zeros(Nsamp,Nsamp)\n", 73 | " for j = 1:Nsamp\n", 74 | " for k = 1:Nsamp\n", 75 | " Zsamp[j,k] = f([Xsamp[j,k]; Ysamp[j,k]])\n", 76 | " end\n", 77 | " end\n", 78 | " contour(Xsamp,Ysamp,Zsamp)\n", 79 | "\n", 80 | " xc = LinRange(-4,3,Nsamp)\n", 81 | " plot(xc,xc.+1,\"y\")\n", 82 | "end\n", 83 | "\n", 84 | "plot_landscape()" 85 | ] 86 | }, 87 | { 88 | "cell_type": "code", 89 | "execution_count": null, 90 | "id": "ef406b43-4ba2-4993-9761-b8aa631cc7d9", 91 | "metadata": {}, 92 | "outputs": [], 93 | "source": [ 94 | "function ip_residual(z, ρ)\n", 95 | " x = z[1:2]\n", 96 | " σ = z[3]\n", 97 | " r = [∇f(x) - ∂c(x)'*sqrt(ρ)*exp(-σ);\n", 98 | " c(x) - sqrt(ρ)exp(σ)]\n", 99 | "end" 100 | ] 101 | }, 102 | { 103 | "cell_type": "code", 104 | "execution_count": null, 105 | "id": "309697fc-2a04-47fb-b590-3062a538fd4a", 106 | "metadata": {}, 107 | "outputs": [], 108 | "source": [ 109 | "function kkt_residual(z)\n", 110 | " x = z[1:2]\n", 111 | " σ = z[3]\n", 112 | " λ = sqrt(ρ)*exp(-σ)\n", 113 | "\n", 114 | " r = [∇f(x) - ∂c(x)'*λ;\n", 115 | " min(λ, 0)\n", 116 | " min(c(x),0)\n", 117 | " λ*c(x)]\n", 118 | "end" 119 | ] 120 | }, 121 | { 122 | "cell_type": "code", 123 | "execution_count": null, 124 | "id": "9435f612-6482-4015-9d48-c2e68217a2ef", 125 | "metadata": {}, 126 | "outputs": [], 127 | "source": [ 128 | "xguess = [-2; 2]\n", 129 | "σguess = 0.0\n", 130 | "z = [xguess; σguess]\n", 131 | "plot_landscape()\n", 132 | "plot(z[1], z[2], \"rx\")" 133 | ] 134 | }, 135 | { 136 | "cell_type": "code", 137 | "execution_count": null, 138 | "id": "16393539-1f14-4703-b938-1423bd5d9de1", 139 | "metadata": {}, 140 | "outputs": [], 141 | "source": [ 142 | "ρ = 1.0\n", 143 | "ip_residual(z,ρ)" 144 | ] 145 | }, 146 | { 147 | "cell_type": "code", 148 | "execution_count": null, 149 | "id": "0dd9b9d0-5615-4459-b420-7dba9e9a83ca", 150 | "metadata": {}, 151 | "outputs": [], 152 | "source": [ 153 | "kkt_residual(z)" 154 | ] 155 | }, 156 | { 157 | "cell_type": "code", 158 | "execution_count": null, 159 | "id": "49c45196-d8ce-4ac5-86d3-192e33ec655a", 160 | "metadata": {}, 161 | "outputs": [], 162 | "source": [ 163 | "function newton_solve(z0,ρ,tol)\n", 164 | "\n", 165 | " #initial guess\n", 166 | " z = z0\n", 167 | " \n", 168 | " #KKT residual\n", 169 | " r = ip_residual(z,ρ)\n", 170 | "\n", 171 | " while norm(r) > tol \n", 172 | " #H = ∇2f(x)\n", 173 | " #C = ∂c(x)\n", 174 | "\n", 175 | " #M = [H sqrt(ρ)*C'*exp(-σ);\n", 176 | " # C -sqrt(ρ)*exp(σ)]\n", 177 | "\n", 178 | " #Newton step\n", 179 | " M = ForwardDiff.jacobian(dz->ip_residual(dz,ρ), z)\n", 180 | " Δz = -M\\r\n", 181 | "\n", 182 | " znew = z + Δz\n", 183 | " rnew = ip_residual(znew,ρ)\n", 184 | "\n", 185 | " #Line search\n", 186 | " b = 0.1\n", 187 | " c = 0.5\n", 188 | " α = 1.0\n", 189 | " while norm(rnew) > (norm(r) + b*α*dot(r,M*Δz)/norm(r))\n", 190 | " α = c*α\n", 191 | " znew = z + α*Δz\n", 192 | " rnew = ip_residual(znew,ρ)\n", 193 | " end\n", 194 | "\n", 195 | " z = znew\n", 196 | " r = rnew\n", 197 | " end\n", 198 | "\n", 199 | " return z\n", 200 | "end" 201 | ] 202 | }, 203 | { 204 | "cell_type": "code", 205 | "execution_count": null, 206 | "id": "2effd81a-aee6-48bc-b692-8c1ebb992195", 207 | "metadata": {}, 208 | "outputs": [], 209 | "source": [ 210 | "z_iter = z" 211 | ] 212 | }, 213 | { 214 | "cell_type": "code", 215 | "execution_count": null, 216 | "id": "b51486c0-a9cd-4a12-a846-32f57fd32c41", 217 | "metadata": {}, 218 | "outputs": [], 219 | "source": [ 220 | "ρ = 1.0e-8 #adjust from ρ=1 to ρ=1e-8 to observe convergence along central path\n", 221 | "z = newton_solve(z_iter[:,end],ρ,1e-10)\n", 222 | "z_iter = [z_iter z]" 223 | ] 224 | }, 225 | { 226 | "cell_type": "code", 227 | "execution_count": null, 228 | "id": "b3ae1497-dd3f-41e2-bfa6-613e38f08ebc", 229 | "metadata": {}, 230 | "outputs": [], 231 | "source": [ 232 | "kkt_residual(z)" 233 | ] 234 | }, 235 | { 236 | "cell_type": "code", 237 | "execution_count": null, 238 | "id": "52d70903-e6dd-47ad-9d46-d88e4f5dc39c", 239 | "metadata": {}, 240 | "outputs": [], 241 | "source": [ 242 | "plot_landscape()\n", 243 | "plot(z_iter[1,:], z_iter[2,:], \"rx\")" 244 | ] 245 | }, 246 | { 247 | "cell_type": "code", 248 | "execution_count": null, 249 | "id": "f1a996bb-9fb5-4419-bfe3-026fafc767b8", 250 | "metadata": { 251 | "scrolled": true 252 | }, 253 | "outputs": [], 254 | "source": [ 255 | "M = ForwardDiff.jacobian(dz->ip_residual(dz,ρ), z)" 256 | ] 257 | }, 258 | { 259 | "cell_type": "code", 260 | "execution_count": null, 261 | "id": "12bb1ec7-945d-4ae7-a37a-dc30db289636", 262 | "metadata": {}, 263 | "outputs": [], 264 | "source": [ 265 | "eigvals(M)" 266 | ] 267 | }, 268 | { 269 | "cell_type": "code", 270 | "execution_count": null, 271 | "id": "774f83a1-6eb2-4e04-8047-8021b158a9e9", 272 | "metadata": {}, 273 | "outputs": [], 274 | "source": [] 275 | } 276 | ], 277 | "metadata": { 278 | "kernelspec": { 279 | "display_name": "Julia 1.10.8", 280 | "language": "julia", 281 | "name": "julia-1.10" 282 | }, 283 | "language_info": { 284 | "file_extension": ".jl", 285 | "mimetype": "application/julia", 286 | "name": "julia", 287 | "version": "1.10.8" 288 | } 289 | }, 290 | "nbformat": 4, 291 | "nbformat_minor": 5 292 | } 293 | -------------------------------------------------------------------------------- /Lecture 6/Lecture 6.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 6/Lecture 6.pdf -------------------------------------------------------------------------------- /Lecture 6/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | julia_version = "1.10.8" 4 | manifest_format = "2.0" 5 | project_hash = "da39a3ee5e6b4b0d3255bfef95601890afd80709" 6 | 7 | [deps] 8 | -------------------------------------------------------------------------------- /Lecture 6/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | -------------------------------------------------------------------------------- /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 | "Δx, Δλ = gauss_newton_step(xguess[:,end],λguess[end])\n", 159 | "α = 1\n", 160 | "while P(xguess[:,end]+α*Δx, λguess[end]+α*Δλ) > P(xguess[:,end], λguess[end]) + 0.01*α*dot(∇P(xguess[:,end], λguess[end]),[Δx; Δλ])\n", 161 | " α = 0.5*α\n", 162 | "end\n", 163 | "xguess = [xguess xguess[:,end]+α*Δx]\n", 164 | "λguess = [λguess λguess[end]+α*Δλ]\n", 165 | "plot_landscape()\n", 166 | "plot(xguess[1,:], xguess[2,:], \"rx\")\n", 167 | "α" 168 | ] 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.10.8", 181 | "language": "julia", 182 | "name": "julia-1.10" 183 | }, 184 | "language_info": { 185 | "file_extension": ".jl", 186 | "mimetype": "application/julia", 187 | "name": "julia", 188 | "version": "1.10.8" 189 | } 190 | }, 191 | "nbformat": 4, 192 | "nbformat_minor": 4 193 | } 194 | -------------------------------------------------------------------------------- /Lecture 6/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.10.8", 192 | "language": "julia", 193 | "name": "julia-1.10" 194 | }, 195 | "language_info": { 196 | "file_extension": ".jl", 197 | "mimetype": "application/julia", 198 | "name": "julia", 199 | "version": "1.10.8" 200 | } 201 | }, 202 | "nbformat": 4, 203 | "nbformat_minor": 4 204 | } 205 | -------------------------------------------------------------------------------- /Lecture 7/Lecture 7.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 7/Lecture 7.pdf -------------------------------------------------------------------------------- /Lecture 7/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | julia_version = "1.10.8" 4 | manifest_format = "2.0" 5 | project_hash = "da39a3ee5e6b4b0d3255bfef95601890afd80709" 6 | 7 | [deps] 8 | -------------------------------------------------------------------------------- /Lecture 7/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | -------------------------------------------------------------------------------- /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.10.8", 203 | "language": "julia", 204 | "name": "julia-1.10" 205 | }, 206 | "language_info": { 207 | "file_extension": ".jl", 208 | "mimetype": "application/julia", 209 | "name": "julia", 210 | "version": "1.10.8" 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/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 8/Lecture 8.pdf -------------------------------------------------------------------------------- /Lecture 8/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | julia_version = "1.10.8" 4 | manifest_format = "2.0" 5 | project_hash = "da39a3ee5e6b4b0d3255bfef95601890afd80709" 6 | 7 | [deps] 8 | -------------------------------------------------------------------------------- /Lecture 8/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | -------------------------------------------------------------------------------- /Lecture 8/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(1.0*I(2))\n", 66 | "R = sparse(0.1*I(1))\n", 67 | "Qn = sparse(1.0*I(2))" 68 | ] 69 | }, 70 | { 71 | "cell_type": "code", 72 | "execution_count": null, 73 | "metadata": {}, 74 | "outputs": [], 75 | "source": [ 76 | "#Cost function\n", 77 | "function J(xhist,uhist)\n", 78 | " cost = 0.5*xhist[:,end]'*Qn*xhist[:,end]\n", 79 | " for k = 1:(N-1)\n", 80 | " cost = cost + 0.5*xhist[:,k]'*Q*xhist[:,k] + 0.5*(uhist[k]'*R*uhist[k])[1]\n", 81 | " end\n", 82 | " return cost\n", 83 | "end" 84 | ] 85 | }, 86 | { 87 | "cell_type": "code", 88 | "execution_count": null, 89 | "metadata": {}, 90 | "outputs": [], 91 | "source": [ 92 | "# Cost\n", 93 | "H = blockdiag(R, kron(I(N-2), blockdiag(Q,R)), Qn);" 94 | ] 95 | }, 96 | { 97 | "cell_type": "code", 98 | "execution_count": null, 99 | "metadata": {}, 100 | "outputs": [], 101 | "source": [ 102 | "# Constraints\n", 103 | "C = kron(I(N-1), [B -I(2)])\n", 104 | "for k = 1:N-2\n", 105 | " C[(k*n).+(1:n), (k*(n+m)-n).+(1:n)] .= A\n", 106 | "end\n", 107 | "d = [-A*x0; zeros(size(C,1)-n)];" 108 | ] 109 | }, 110 | { 111 | "cell_type": "code", 112 | "execution_count": null, 113 | "metadata": {}, 114 | "outputs": [], 115 | "source": [ 116 | "# Solve the linear system\n", 117 | "y = [H C'; C zeros(size(C,1),size(C,1))]\\[zeros(size(H,1)); d]\n", 118 | "\n", 119 | "# Get state history\n", 120 | "z = y[1:size(H,1)] # states and controls [u0,x1,u1,...,xN]\n", 121 | "Z = reshape(z,n+m,N-1)\n", 122 | "xhist = Z[m+1:end,:]\n", 123 | "xhist = [x0 xhist]\n", 124 | "\n", 125 | "# Get control history\n", 126 | "uhist = Z[1,:];" 127 | ] 128 | }, 129 | { 130 | "cell_type": "code", 131 | "execution_count": null, 132 | "metadata": {}, 133 | "outputs": [], 134 | "source": [ 135 | "# Plot x1 vs. x2, u vs. t, x vs. t, etc.\n", 136 | "times = range(0,h*(N-1), step=h)\n", 137 | "plot(times,xhist[1,:], label=\"position\")\n", 138 | "plot(times,xhist[2,:], label=\"velocity\")\n", 139 | "xlabel(\"time\")\n", 140 | "legend()" 141 | ] 142 | }, 143 | { 144 | "cell_type": "code", 145 | "execution_count": null, 146 | "metadata": {}, 147 | "outputs": [], 148 | "source": [ 149 | "plot(times[1:end-1], uhist, label=\"control\")\n", 150 | "xlabel(\"Time\")\n", 151 | "legend()" 152 | ] 153 | }, 154 | { 155 | "cell_type": "code", 156 | "execution_count": null, 157 | "metadata": {}, 158 | "outputs": [], 159 | "source": [ 160 | "J(xhist,uhist)" 161 | ] 162 | }, 163 | { 164 | "cell_type": "code", 165 | "execution_count": null, 166 | "metadata": {}, 167 | "outputs": [], 168 | "source": [] 169 | }, 170 | { 171 | "cell_type": "code", 172 | "execution_count": null, 173 | "metadata": {}, 174 | "outputs": [], 175 | "source": [] 176 | } 177 | ], 178 | "metadata": { 179 | "kernelspec": { 180 | "display_name": "Julia 1.10.8", 181 | "language": "julia", 182 | "name": "julia-1.10" 183 | }, 184 | "language_info": { 185 | "file_extension": ".jl", 186 | "mimetype": "application/julia", 187 | "name": "julia", 188 | "version": "1.10.8" 189 | } 190 | }, 191 | "nbformat": 4, 192 | "nbformat_minor": 4 193 | } 194 | -------------------------------------------------------------------------------- /Lecture 8/lqr-riccati.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": null, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": [ 9 | "import Pkg; Pkg.activate(@__DIR__); Pkg.instantiate()" 10 | ] 11 | }, 12 | { 13 | "cell_type": "code", 14 | "execution_count": null, 15 | "metadata": {}, 16 | "outputs": [], 17 | "source": [ 18 | "using LinearAlgebra\n", 19 | "using PyPlot\n", 20 | "using ControlSystems" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": null, 26 | "metadata": {}, 27 | "outputs": [], 28 | "source": [ 29 | "# Discrete dynamics\n", 30 | "h = 0.1 # time step\n", 31 | "A = [1 h; 0 1]\n", 32 | "B = [0.5*h*h; h]" 33 | ] 34 | }, 35 | { 36 | "cell_type": "code", 37 | "execution_count": null, 38 | "metadata": {}, 39 | "outputs": [], 40 | "source": [ 41 | "n = 2 # number of state\n", 42 | "m = 1 # number of controls\n", 43 | "Tfinal = 10.0 # final time\n", 44 | "N = Int(Tfinal/h)+1 # number of time steps\n", 45 | "thist = Array(range(0,h*(N-1), step=h));" 46 | ] 47 | }, 48 | { 49 | "cell_type": "code", 50 | "execution_count": null, 51 | "metadata": {}, 52 | "outputs": [], 53 | "source": [ 54 | "# Initial conditions\n", 55 | "x0 = [1.0; 0]" 56 | ] 57 | }, 58 | { 59 | "cell_type": "code", 60 | "execution_count": null, 61 | "metadata": {}, 62 | "outputs": [], 63 | "source": [ 64 | "# Cost weights\n", 65 | "Q = Array(1.0*I(2))\n", 66 | "R = 0.1 #Array(1.0*I(1))\n", 67 | "Qn = Array(1.0*I(2))" 68 | ] 69 | }, 70 | { 71 | "cell_type": "code", 72 | "execution_count": null, 73 | "metadata": {}, 74 | "outputs": [], 75 | "source": [ 76 | "#Cost function\n", 77 | "function J(xhist,uhist)\n", 78 | " cost = 0.5*xhist[:,end]'*Qn*xhist[:,end]\n", 79 | " for k = 1:(N-1)\n", 80 | " cost = cost + 0.5*xhist[:,k]'*Q*xhist[:,k] + 0.5*(uhist[k]'*R*uhist[k])[1]\n", 81 | " end\n", 82 | " return cost\n", 83 | "end" 84 | ] 85 | }, 86 | { 87 | "cell_type": "code", 88 | "execution_count": null, 89 | "metadata": {}, 90 | "outputs": [], 91 | "source": [ 92 | "P = zeros(n,n,N)\n", 93 | "K = zeros(m,n,N-1)\n", 94 | "\n", 95 | "P[:,:,N] .= Qn\n", 96 | "\n", 97 | "#Backward Riccati recursion\n", 98 | "for k = (N-1):-1:1\n", 99 | " K[:,:,k] .= (R + B'*P[:,:,k+1]*B)\\(B'*P[:,:,k+1]*A)\n", 100 | " P[:,:,k] .= Q + A'*P[:,:,k+1]*(A-B*K[:,:,k])\n", 101 | "end\n", 102 | "\n", 103 | "#Forward rollout starting at x0\n", 104 | "xhist = zeros(n,N)\n", 105 | "xhist[:,1] = x0\n", 106 | "uhist = zeros(m,N-1)\n", 107 | "for k = 1:(N-1)\n", 108 | " uhist[:,k] .= -K[:,:,k]*xhist[:,k]\n", 109 | " xhist[:,k+1] .= A*xhist[:,k] + B*uhist[k]\n", 110 | "end" 111 | ] 112 | }, 113 | { 114 | "cell_type": "code", 115 | "execution_count": null, 116 | "metadata": {}, 117 | "outputs": [], 118 | "source": [ 119 | "# Plot x1 vs. x2, u vs. t, x vs. t, etc.\n", 120 | "times = range(0,h*(N-1), step=h)\n", 121 | "plot(times,xhist[1,:], label=\"position\")\n", 122 | "plot(times,xhist[2,:], label=\"velocity\")\n", 123 | "xlabel(\"time\")\n", 124 | "legend()" 125 | ] 126 | }, 127 | { 128 | "cell_type": "code", 129 | "execution_count": null, 130 | "metadata": {}, 131 | "outputs": [], 132 | "source": [ 133 | "plot(times[1:end-1], uhist[1,:], label=\"control\")\n", 134 | "xlabel(\"Time\")\n", 135 | "legend()" 136 | ] 137 | }, 138 | { 139 | "cell_type": "code", 140 | "execution_count": null, 141 | "metadata": {}, 142 | "outputs": [], 143 | "source": [ 144 | "J(xhist,uhist)" 145 | ] 146 | }, 147 | { 148 | "cell_type": "code", 149 | "execution_count": null, 150 | "metadata": {}, 151 | "outputs": [], 152 | "source": [ 153 | "#Forward rollout with random noise\n", 154 | "xhist = zeros(n,N)\n", 155 | "xhist[:,1] = x0 + 10.0*randn(2) #Can start anywhere\n", 156 | "uhist = zeros(m,N-1)\n", 157 | "for k = 1:(N-1)\n", 158 | " uhist[:,k] .= -K[:,:,k]*xhist[:,k]\n", 159 | " xhist[:,k+1] .= A*xhist[:,k] + B*uhist[k] + 0.1*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.10.8", 224 | "language": "julia", 225 | "name": "julia-1.10" 226 | }, 227 | "language_info": { 228 | "file_extension": ".jl", 229 | "mimetype": "application/julia", 230 | "name": "julia", 231 | "version": "1.10.8" 232 | } 233 | }, 234 | "nbformat": 4, 235 | "nbformat_minor": 4 236 | } 237 | -------------------------------------------------------------------------------- /Lecture 9/Lecture 9.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Optimal-Control-16-745/lecture-notebooks/e7c23f67d432c2656400d2c0af1f9ee78f01bb2f/Lecture 9/Lecture 9.pdf -------------------------------------------------------------------------------- /Lecture 9/Manifest.toml: -------------------------------------------------------------------------------- 1 | # This file is machine-generated - editing it directly is not advised 2 | 3 | julia_version = "1.10.8" 4 | manifest_format = "2.0" 5 | project_hash = "da39a3ee5e6b4b0d3255bfef95601890afd80709" 6 | 7 | [deps] 8 | -------------------------------------------------------------------------------- /Lecture 9/Project.toml: -------------------------------------------------------------------------------- 1 | [deps] 2 | -------------------------------------------------------------------------------- /Lecture 9/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.10.8", 372 | "language": "julia", 373 | "name": "julia-1.10" 374 | }, 375 | "language_info": { 376 | "file_extension": ".jl", 377 | "mimetype": "application/julia", 378 | "name": "julia", 379 | "version": "1.10.8" 380 | } 381 | }, 382 | "nbformat": 4, 383 | "nbformat_minor": 4 384 | } 385 | -------------------------------------------------------------------------------- /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 2025. 4 | 5 |

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

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