├── .gitignore ├── Documentation ├── Direct-Methods-SLP-Presentation.pdf └── Direct-Methods-SLP-Report.pdf ├── LICENSE ├── Optimal-Control-Problems ├── Bryson_Denham │ ├── Bryson_Denham_collocation.m │ ├── Bryson_Denham_multiple_shooting.m │ ├── Bryson_Denham_single_shooting.m │ └── results │ │ ├── optimal_sol_collocation.png │ │ ├── optimal_sol_multiple_shooting.png │ │ └── optimal_sol_single_shooting.png └── Double_Integrator │ ├── double_integrator_collocation.m │ ├── double_integrator_multiple_shooting.m │ ├── double_integrator_single_shooting.m │ └── results │ ├── optimal_sol_collocation.png │ ├── optimal_sol_multiple_shooting.png │ └── optimal_sol_single_shooting.png └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | 131 | # Windows .ini file 132 | desktop.ini 133 | 134 | # Windows default autosave extension 135 | *.asv 136 | 137 | # OSX / *nix default autosave extension 138 | *.m~ 139 | 140 | # Compiled MEX binaries (all platforms) 141 | *.mex* 142 | 143 | # Packaged app and toolbox files 144 | *.mlappinstall 145 | *.mltbx 146 | 147 | # Generated helpsearch folders 148 | helpsearch*/ 149 | 150 | # Simulink code generation folders 151 | slprj/ 152 | sccprj/ 153 | 154 | # Matlab code generation folders 155 | codegen/ 156 | 157 | # Simulink autosave extension 158 | *.autosave 159 | 160 | # Simulink cache files 161 | *.slxc 162 | 163 | # Octave session info 164 | octave-workspace 165 | -------------------------------------------------------------------------------- /Documentation/Direct-Methods-SLP-Presentation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nakulrandad/Direct-Methods-for-Optimal-Control/1ab440475578b478c0818ddcc8fb9b49e0931de6/Documentation/Direct-Methods-SLP-Presentation.pdf -------------------------------------------------------------------------------- /Documentation/Direct-Methods-SLP-Report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nakulrandad/Direct-Methods-for-Optimal-Control/1ab440475578b478c0818ddcc8fb9b49e0931de6/Documentation/Direct-Methods-SLP-Report.pdf -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Nakul Randad 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Optimal-Control-Problems/Bryson_Denham/Bryson_Denham_collocation.m: -------------------------------------------------------------------------------- 1 | %-------------------------------------------------------------------------- 2 | % Bryson_Denham_collocation.m 3 | % This file solves the Bryson-Denham problem by 4 | % direct collocation method using Casadi 5 | %-------------------------------------------------------------------------- 6 | % Primary Contributor: Nakul Randad, Indian Institute of Technology Bombay 7 | %-------------------------------------------------------------------------- 8 | % CasADi v3.5.5 9 | import casadi.* 10 | 11 | close all 12 | % clc 13 | 14 | %% Set up collocation points 15 | % Degree of interpolating polynomial 16 | d = 3; 17 | 18 | % Get collocation points 19 | tau = collocation_points(d, 'legendre'); 20 | 21 | % Collocation linear maps 22 | [C,D,B] = collocation_coeff(tau); 23 | 24 | %% Declare decision variables 25 | T = 1; % Time horizon 26 | limit = 1/9; % position limit 27 | 28 | % Declare model variables 29 | x1 = SX.sym('x1'); % position 30 | x2 = SX.sym('x2'); % velocity 31 | x = [x1; x2]; 32 | u = SX.sym('u'); 33 | 34 | %% Dynamics and Objective 35 | % Model equations 36 | xdot = [x2; u]; 37 | 38 | % Objective term 39 | L = u^2/2; 40 | 41 | % Continuous time dynamics 42 | f = Function('f', {x, u}, {xdot, L}); 43 | 44 | %% Control discretization 45 | N = 100; % number of control intervals 46 | h = T/N; 47 | 48 | %% Set up solver 49 | % Start with an empty NLP 50 | 51 | opti = Opti(); 52 | J = 0; 53 | 54 | % Initial conditions 55 | Xk = opti.variable(2); 56 | opti.subject_to(Xk==[0; 1]); 57 | opti.set_initial(Xk, [0; 1]); 58 | 59 | % Collect all states/controls 60 | Xs = {Xk}; 61 | Us = {}; 62 | cost = []; 63 | 64 | % Formulate the NLP 65 | for k=0:N-1 66 | % New NLP variable for the control 67 | Uk = opti.variable(); 68 | Us{end+1} = Uk; 69 | opti.set_initial(Uk, 0); 70 | 71 | % Decision variables for helper states at each collocation point 72 | Xc = opti.variable(2, d); 73 | opti.subject_to(Xc(1,:) <= limit); % inequality constraint on state 74 | opti.set_initial(Xc, repmat([0;0],1,d)); 75 | 76 | % Evaluate ODE right-hand-side at all helper states 77 | [ode, quad] = f(Xc, Uk); 78 | 79 | % Add contribution to quadrature function 80 | int_L = quad*B*h; 81 | J = J + int_L; 82 | cost = [cost; J]; 83 | 84 | % Get interpolating points of collocation polynomial 85 | Z = [Xk Xc]; 86 | 87 | % Get slope of interpolating polynomial (normalized) 88 | Pidot = Z*C; 89 | % Match with ODE right-hand-side 90 | opti.subject_to(Pidot == h*ode); 91 | 92 | % State at end of collocation interval 93 | Xk_end = Z*D; 94 | 95 | % New decision variable for state at end of interval 96 | Xk = opti.variable(2); 97 | Xs{end+1} = Xk; 98 | opti.subject_to(Xk(1) <= limit);% inequality constraint on state 99 | opti.set_initial(Xk, [0;0]); 100 | 101 | % Continuity constraints 102 | opti.subject_to(Xk_end==Xk) 103 | end 104 | 105 | % Boundary conditions 106 | opti.subject_to(Xs{end}(1)==0); 107 | opti.subject_to(Xs{end}(2)==-1); 108 | 109 | %% Optimisation solver 110 | 111 | Xs = [Xs{:}]; 112 | Us = [Us{:}]; 113 | 114 | opti.minimize(J); % minimise the objective function 115 | 116 | opti.solver('ipopt'); % backend NLP solver 117 | 118 | tic 119 | sol = opti.solve(); % Solve actual problem 120 | toc 121 | 122 | x_opt = sol.value(Xs); 123 | u_opt = sol.value(Us); 124 | cost_opt = sol.value(cost); 125 | 126 | %% Post-processing 127 | time = linspace(0, T, N+1); 128 | t = tiledlayout(2,2); 129 | t.Padding = 'compact'; 130 | t.TileSpacing = 'compact'; 131 | 132 | nexttile 133 | hold on 134 | plot(time,x_opt(1,:),'b','LineWidth',1); 135 | yline(limit,'k--','LineWidth',1); 136 | hold off 137 | ylim([-inf, limit*1.05]) 138 | ylabel('Position'); 139 | xlabel('Time [s]'); 140 | legend('x','$x < \frac{1}{9}$','Interpreter','latex','Location', 'South'); 141 | 142 | nexttile 143 | plot(time, x_opt(2,:),'Color',[0, 0.5, 0],'LineWidth',1); 144 | ylabel('Speed'); 145 | xlabel('Time [s]'); 146 | legend('v','Location', 'South') 147 | 148 | nexttile 149 | plot(time, [u_opt nan],'r','LineWidth',1); 150 | xlabel('Time [s]'); 151 | ylabel('Thrust'); 152 | legend('u','Location', 'South'); 153 | 154 | nexttile 155 | plot(time,[cost_opt; nan],'--','LineWidth',1); 156 | xlabel('Time [s]'); 157 | ylabel('Objective'); 158 | legend('$\frac{1}{2} \int u^2$','Interpreter','latex','Location', 'South'); 159 | 160 | % To print the figure 161 | % print('./results/optimal_sol_collocation','-dpng') -------------------------------------------------------------------------------- /Optimal-Control-Problems/Bryson_Denham/Bryson_Denham_multiple_shooting.m: -------------------------------------------------------------------------------- 1 | %-------------------------------------------------------------------------- 2 | % Bryson_Denham_multiple_shooting.m 3 | % This file solves the Bryson-Denham problem by 4 | % multiple-shooting method using Casadi 5 | %-------------------------------------------------------------------------- 6 | % Primary Contributor: Nakul Randad, Indian Institute of Technology Bombay 7 | %-------------------------------------------------------------------------- 8 | % CasADi v3.5.5 9 | import casadi.* 10 | 11 | close all 12 | % clc 13 | 14 | %% Setting up the problem 15 | N = 100; % number of control intervals 16 | opti = casadi.Opti(); % Optimization problem 17 | 18 | %% Declare decision variables 19 | X = opti.variable(2,N+1); % state trajectory 20 | pos = X(1,:); 21 | speed = X(2,:); 22 | U = opti.variable(1,N); % control trajectory (throttle) 23 | T = 1; % final time 24 | limit = 1/9; % position limit 25 | time = linspace(0, T, N+1)'; % time horizon 26 | 27 | %% Set up the objective 28 | L = U.^2/2; % integrand 29 | dt = T/N; % length of a control interval 30 | cost = []; 31 | obj = 0; 32 | for i = 1:N 33 | obj = obj + L(i)*dt; 34 | cost = [cost;obj]; 35 | end 36 | opti.minimize(obj); % minimize objective 37 | 38 | %% System dynamics 39 | f = @(x,u) [x(2);u]; % dx/dt = f(x,u) 40 | 41 | %% Numerical integration and constraint to make zero gap 42 | for k=1:N % loop over control intervals 43 | % Runge-Kutta 4 integration 44 | k1 = f(X(:,k), U(:,k)); 45 | k2 = f(X(:,k)+dt/2*k1, U(:,k)); 46 | k3 = f(X(:,k)+dt/2*k2, U(:,k)); 47 | k4 = f(X(:,k)+dt*k3, U(:,k)); 48 | x_next = X(:,k) + dt/6*(k1+2*k2+2*k3+k4); 49 | opti.subject_to(X(:,k+1)==x_next); % close the gaps 50 | end 51 | 52 | %% Path constraints 53 | opti.subject_to(pos<=limit); % position limit 54 | 55 | %% Boundary conditions 56 | opti.subject_to(pos(1)==0); % start at position 0 57 | opti.subject_to(speed(1)==1); 58 | opti.subject_to(pos(N+1)==0); % finish line at position 0 59 | opti.subject_to(speed(N+1)==-1); 60 | 61 | %% Initial guess 62 | opti.set_initial(speed, 1); 63 | opti.set_initial(pos, 0); 64 | 65 | %% Solver set up 66 | opti.solver('ipopt'); % set numerical backend 67 | tic 68 | sol = opti.solve(); % actual solve 69 | toc 70 | 71 | %% Post-processing 72 | t = tiledlayout(2,2); 73 | t.Padding = 'compact'; 74 | t.TileSpacing = 'compact'; 75 | 76 | nexttile 77 | hold on 78 | plot(time,sol.value(pos),'b','LineWidth',1); 79 | yline(limit,'k--','LineWidth',1); 80 | hold off 81 | ylim([-inf, limit*1.05]) 82 | ylabel('Position'); 83 | xlabel('Time [s]'); 84 | legend('x','$x < \frac{1}{9}$','Interpreter','latex','Location', 'South'); 85 | 86 | nexttile 87 | plot(time,sol.value(speed),'Color',[0, 0.5, 0],'LineWidth',1); 88 | ylabel('Speed'); 89 | xlabel('Time [s]'); 90 | legend('v','Location', 'South') 91 | 92 | nexttile 93 | stairs(time(1:end-1),sol.value(U),'r','LineWidth',1); 94 | xlabel('Time [s]'); 95 | ylabel('Thrust'); 96 | legend('u','Location', 'South'); 97 | 98 | nexttile 99 | plot(time(1:end-1),sol.value(cost),'--','LineWidth',1); 100 | xlabel('Time [s]'); 101 | ylabel('Objective'); 102 | legend('$\frac{1}{2} \int u^2$','Interpreter','latex','Location', 'South'); 103 | 104 | % To print the figure 105 | % print('./results/optimal_sol_multiple_shooting','-dpng') -------------------------------------------------------------------------------- /Optimal-Control-Problems/Bryson_Denham/Bryson_Denham_single_shooting.m: -------------------------------------------------------------------------------- 1 | %-------------------------------------------------------------------------- 2 | % Bryson_Denham_single_shooting.m 3 | % This file solves the Bryson-Denham problem by single-shooting method 4 | % using fmincon 5 | % (employs the trapezodial rule with composite trapezoidal quadrature) 6 | %-------------------------------------------------------------------------- 7 | % Primary Contributor: Nakul Randad, Indian Institute of Technology Bombay 8 | %-------------------------------------------------------------------------- 9 | 10 | soln = method(); 11 | 12 | % actual implementation 13 | function soln = method 14 | % problem parameters 15 | p.ns = 2; p.nu = 1; % number of states and controls 16 | p.t0 = 0; p.tf = 1; % time horizon 17 | p.y10 = 0; p.y1f = 0; p.y20 = 1; p.y2f = -1; % boundary conditions 18 | p.l = 1/9; 19 | 20 | % direct transcription parameters 21 | % p.nt = 20; % number of node points 22 | p.nt = 100; % number of node points 23 | p.t = linspace(p.t0,p.tf,p.nt)'; % time horizon 24 | p.h = diff(p.t); % step size 25 | 26 | % discretized variable indices in x = [y1,y2,u]; 27 | p.y1i = 1:p.nt; p.y2i = p.nt+1:2*p.nt; p.ui = 2*p.nt+1:3*p.nt; 28 | x0 = zeros(p.nt*(p.ns+p.nu),1); % initial guess (all zeros) 29 | options = optimoptions(@fmincon,'display','iter','MaxFunEvals',1e5); % options 30 | 31 | % solve the problem 32 | tic 33 | x = fmincon(@(x) objective(x,p),x0,[],[],[],[],[],[],@(x) constraints(x,p),options); 34 | toc 35 | 36 | % obtain the optimal solution 37 | y1 = x(p.y1i); y2 = x(p.y2i); u = x(p.ui); % extract 38 | soln.y1 = y1; soln.y2 = y2; soln.u = u; soln.p = p; 39 | 40 | % plots 41 | showPlot(y1,y2,u,p) 42 | end 43 | 44 | % objective function 45 | function f = objective(x,p) 46 | u = x(p.ui); % extract 47 | L = u.^2/2; % integrand 48 | f = trapz(p.t,L); % calculate objective 49 | end 50 | 51 | % constraint function 52 | function [c,ceq] = constraints(x,p) 53 | y1 = x(p.y1i); y2 = x(p.y2i); u = x(p.ui); % extract 54 | Y = [y1,y2]; F = [y2,u]; % create matrices (p.nt x p.ns) 55 | ceq1 = y1(1) - p.y10; % initial state conditions 56 | ceq2 = y2(1) - p.y20; 57 | ceq3 = y1(end) - p.y1f; % final state conditions 58 | ceq4 = y2(end) - p.y2f; 59 | 60 | % integrate using trapezoidal quadrature 61 | ceq5 = Y(2:p.nt,1) - Y(1:p.nt-1,1) - p.h/2.*( F(1:p.nt-1,1) + F(2:p.nt,1) ); 62 | ceq6 = Y(2:p.nt,2) - Y(1:p.nt-1,2) - p.h/2.*( F(1:p.nt-1,2) + F(2:p.nt,2) ); 63 | c1 = y1 - p.l; % path constraints 64 | 65 | c = c1; ceq = [ceq1;ceq2;ceq3;ceq4;ceq5;ceq6]; % combine constraints 66 | end 67 | 68 | % plotting function 69 | function showPlot(y1,y2,u,p) 70 | close all 71 | t = tiledlayout(2,2); 72 | t.Padding = 'compact'; 73 | t.TileSpacing = 'compact'; 74 | 75 | nexttile 76 | hold on 77 | plot(p.t,y1,'b','LineWidth',1); 78 | yline(p.l,'k--','LineWidth',1); 79 | scatter(p.t0,p.y10,[],'g','filled'); 80 | scatter(p.tf,p.y1f,[],'r','filled'); 81 | hold off 82 | ylim([-inf, p.l*1.05]) 83 | ylabel('Position'); 84 | xlabel('Time [s]'); 85 | legend('$y1$','$y1 < \frac{1}{9}$',"$y1_0$","$y1_f$",'Interpreter','latex','Location', 'South'); 86 | 87 | nexttile 88 | hold on 89 | plot(p.t,y2,'Color',[0, 0.5, 0],'LineWidth',1); 90 | scatter(p.t0,p.y20,[],'g','filled'); 91 | scatter(p.tf,p.y2f,[],'r','filled'); 92 | hold off 93 | ylabel('Speed'); 94 | xlabel('Time [s]'); 95 | legend('$y2$',"$y2_0$","$y2_f$",'Interpreter','latex','Location', 'South') 96 | 97 | nexttile(3,[1,2]) 98 | stairs(p.t,u,'r','LineWidth',1); 99 | xlabel('Time [s]'); 100 | ylabel('Thrust'); 101 | legend('u','Location', 'South'); 102 | 103 | % To print the figure 104 | % print('./results/optimal_sol_single_shooting','-dpng') 105 | end -------------------------------------------------------------------------------- /Optimal-Control-Problems/Bryson_Denham/results/optimal_sol_collocation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nakulrandad/Direct-Methods-for-Optimal-Control/1ab440475578b478c0818ddcc8fb9b49e0931de6/Optimal-Control-Problems/Bryson_Denham/results/optimal_sol_collocation.png -------------------------------------------------------------------------------- /Optimal-Control-Problems/Bryson_Denham/results/optimal_sol_multiple_shooting.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nakulrandad/Direct-Methods-for-Optimal-Control/1ab440475578b478c0818ddcc8fb9b49e0931de6/Optimal-Control-Problems/Bryson_Denham/results/optimal_sol_multiple_shooting.png -------------------------------------------------------------------------------- /Optimal-Control-Problems/Bryson_Denham/results/optimal_sol_single_shooting.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nakulrandad/Direct-Methods-for-Optimal-Control/1ab440475578b478c0818ddcc8fb9b49e0931de6/Optimal-Control-Problems/Bryson_Denham/results/optimal_sol_single_shooting.png -------------------------------------------------------------------------------- /Optimal-Control-Problems/Double_Integrator/double_integrator_collocation.m: -------------------------------------------------------------------------------- 1 | %-------------------------------------------------------------------------- 2 | % double_integrator_collocation.m 3 | % This file solves the double integrator bang-bang problem by 4 | % direct collocation method using Casadi 5 | %-------------------------------------------------------------------------- 6 | % Primary Contributor: Nakul Randad, Indian Institute of Technology Bombay 7 | %-------------------------------------------------------------------------- 8 | % CasADi v3.5.5 9 | import casadi.* 10 | 11 | close all 12 | % clc 13 | 14 | %% Set up collocation points 15 | % Degree of interpolating polynomial 16 | d = 3; 17 | 18 | % Get collocation points 19 | tau = collocation_points(d, 'legendre'); 20 | 21 | % Collocation linear maps 22 | [C,D,B] = collocation_coeff(tau); 23 | 24 | %% Declare decision variables 25 | x1 = SX.sym('x1'); % position 26 | x2 = SX.sym('x2'); % velocity 27 | x = [x1; x2]; 28 | u = SX.sym('u'); % control 29 | 30 | %% Dynamics and Objective 31 | % Model equations 32 | xdot = [x2; u]; 33 | 34 | % Continuous time dynamics 35 | f = Function('f', {x, u}, {xdot}); 36 | 37 | %% Discretization 38 | N = 100; % number of control intervals 39 | 40 | %% Set up solver 41 | % Start with an empty NLP 42 | opti = Opti(); 43 | 44 | T = opti.variable(); % Time horizon 45 | opti.set_initial(T, 1); % Inital guess 46 | opti.subject_to(T>0); % Time must be positive 47 | h = T/N; 48 | 49 | % Initial conditions 50 | Xk = opti.variable(2); 51 | opti.subject_to(Xk==[10; 0]); 52 | opti.set_initial(Xk, [10; 0]); 53 | 54 | % Collect all states/controls 55 | Xs = {Xk}; 56 | Us = {}; 57 | J = 0; 58 | 59 | % Formulate the NLP 60 | for k=1:N 61 | % New NLP variable for the control 62 | Uk = opti.variable(); 63 | Us{end+1} = Uk; 64 | u_lim = 1; 65 | opti.subject_to(-u_lim<=Uk); 66 | opti.subject_to(Uk<=u_lim); 67 | opti.set_initial(Uk, 0); 68 | 69 | % Decision variables for helper states at each collocation point 70 | Xc = opti.variable(2, d); 71 | opti.set_initial(Xc, repmat([0;0],1,d)); 72 | 73 | % Evaluate ODE right-hand-side at all helper states 74 | [ode] = f(Xc, Uk); 75 | 76 | % Add contribution to quadrature function 77 | int_L = ones(1,3)*B*h; 78 | J = J + int_L; 79 | 80 | % Get interpolating points of collocation polynomial 81 | Z = [Xk Xc]; 82 | 83 | % Get slope of interpolating polynomial (normalized) 84 | Pidot = Z*C; 85 | % Match with ODE right-hand-side 86 | opti.subject_to(Pidot == h*ode); 87 | 88 | % State at end of collocation interval 89 | Xk_end = Z*D; 90 | 91 | % New decision variable for state at end of interval 92 | Xk = opti.variable(2); 93 | Xs{end+1} = Xk; 94 | opti.set_initial(Xk, [0;0]); 95 | 96 | % Continuity constraints 97 | opti.subject_to(Xk_end==Xk) 98 | end 99 | 100 | % Boundary conditions at finish line 101 | opti.subject_to(Xs{end}(1)==0); 102 | opti.subject_to(Xs{end}(2)==0); 103 | 104 | %% Optimisation solver 105 | Xs = [Xs{:}]; 106 | Us = [Us{:}]; 107 | 108 | opti.minimize(T); 109 | 110 | opti.solver('ipopt'); 111 | 112 | tic 113 | sol = opti.solve(); 114 | toc 115 | 116 | x_opt = sol.value(Xs); 117 | u_opt = sol.value(Us); 118 | 119 | %% Post-processing 120 | time = linspace(0, sol.value(T), N+1)'; 121 | 122 | t = tiledlayout(2,1); 123 | t.Padding = 'compact'; 124 | t.TileSpacing = 'compact'; 125 | title(t, ['Optimal time taken is ', num2str(time(end)), ' secs.']); 126 | 127 | nexttile 128 | hold on 129 | grid on 130 | plot(time,x_opt(1,:),'b','LineWidth',1); 131 | plot(time,x_opt(2,:),'Color',[0, 0.5, 0],'LineWidth',1); 132 | legend('Pos','Vel'); 133 | xlabel("Time (in sec)"); 134 | xlim([0,time(end)]); 135 | hold off 136 | 137 | nexttile 138 | grid on 139 | plot(time, [u_opt nan],'r--','LineWidth',1); 140 | legend('u'); 141 | ylabel("Acceleration"); 142 | xlabel("Time (in sec)"); 143 | xlim([0,time(end)]); 144 | ylim([-u_lim,u_lim]*1.2) 145 | 146 | % To print the figure 147 | % print(".\results\optimal_sol_collocation",'-dpng') -------------------------------------------------------------------------------- /Optimal-Control-Problems/Double_Integrator/double_integrator_multiple_shooting.m: -------------------------------------------------------------------------------- 1 | %-------------------------------------------------------------------------- 2 | % double_integrator_multiple_shooting.m 3 | % This file solves the double integrator bang-bang problem by 4 | % multiple-shooting method using Casadi 5 | %-------------------------------------------------------------------------- 6 | % Primary Contributor: Nakul Randad, Indian Institute of Technology Bombay 7 | %-------------------------------------------------------------------------- 8 | % CasADi v3.5.5 9 | import casadi.* 10 | 11 | close all 12 | % clc 13 | 14 | %% Setting up the problem 15 | N = 100; % number of control intervals 16 | opti = casadi.Opti(); % Optimization problem 17 | 18 | %% Declare decision variables 19 | X = opti.variable(2,N+1); % state trajectory 20 | pos = X(1,:); 21 | speed = X(2,:); 22 | U = opti.variable(1,N); % control trajectory (throttle) 23 | T = opti.variable(); % final time 24 | 25 | %% Set up the objective 26 | opti.minimize(T); % minimize time 27 | 28 | %% System dynamics 29 | f = @(x,u) [x(2);u]; % dx/dt = f(x,u) 30 | 31 | %% Numerical integration and constraint to make zero gap 32 | dt = T/N; % length of a control interval 33 | for k=1:N % loop over control intervals 34 | % Runge-Kutta 4 integration 35 | k1 = f(X(:,k), U(:,k)); 36 | k2 = f(X(:,k)+dt/2*k1, U(:,k)); 37 | k3 = f(X(:,k)+dt/2*k2, U(:,k)); 38 | k4 = f(X(:,k)+dt*k3, U(:,k)); 39 | x_next = X(:,k) + dt/6*(k1+2*k2+2*k3+k4); 40 | opti.subject_to(X(:,k+1)==x_next); % close the gaps 41 | end 42 | 43 | %% Path constraints 44 | u_lim = 1; 45 | opti.subject_to(-u_lim<=U); 46 | opti.subject_to(U<=u_lim); 47 | opti.subject_to(T>0); % Time must be positive 48 | 49 | %% Boundary conditions 50 | opti.subject_to(pos(1)==10); % start position 51 | opti.subject_to(speed(1)==0); 52 | opti.subject_to(pos(N+1)==0); % finish line 53 | opti.subject_to(speed(N+1)==0); 54 | 55 | %% Initial guess 56 | opti.set_initial(speed, 0); 57 | opti.set_initial(pos, 0); 58 | opti.set_initial(T, 1); 59 | 60 | %% Solver set up 61 | opti.solver('ipopt'); % set numerical backend 62 | tic 63 | sol = opti.solve(); % actual solve 64 | toc 65 | 66 | %% Post-processing 67 | time = linspace(0, sol.value(T), N+1)'; 68 | 69 | t = tiledlayout(2,1); 70 | t.Padding = 'compact'; 71 | t.TileSpacing = 'compact'; 72 | title(t, ['Optimal time taken is ', num2str(time(end)), ' secs.']); 73 | 74 | nexttile 75 | hold on 76 | grid on 77 | plot(time,sol.value(pos),'b','LineWidth',1); 78 | plot(time,sol.value(speed),'Color',[0, 0.5, 0],'LineWidth',1); 79 | legend('Pos','Vel'); 80 | xlabel("Time (in sec)"); 81 | xlim([0,time(end)]); 82 | hold off 83 | 84 | nexttile 85 | grid on 86 | stairs(time(1:end-1),sol.value(U),'r--','LineWidth',1); 87 | legend('u'); 88 | ylabel("Acceleration"); 89 | xlabel("Time (in sec)"); 90 | xlim([0,time(end)]); 91 | ylim([-u_lim,u_lim]*1.2) 92 | 93 | % To print the figure 94 | % print('./results/optimal_sol_multiple_shooting','-dpng') -------------------------------------------------------------------------------- /Optimal-Control-Problems/Double_Integrator/double_integrator_single_shooting.m: -------------------------------------------------------------------------------- 1 | %-------------------------------------------------------------------------- 2 | % double_integrator_single_shooting.m 3 | % This file solves the double integrator bang-bang problem using a 4 | % single-step method using fmincon 5 | % (employs the trapezodial rule with composite trapezoidal quadrature) 6 | %-------------------------------------------------------------------------- 7 | % Primary Contributor: Nakul Randad, Indian Institute of Technology Bombay 8 | %-------------------------------------------------------------------------- 9 | 10 | soln = method(); 11 | 12 | % actual implementation 13 | function soln = method 14 | % problem parameters 15 | p.ns = 2; p.nu = 1; % number of states and controls 16 | p.t0 = 0; % time starts at '0' 17 | p.y10 = 10; p.y1f = 0; p.y20 = 0; p.y2f = 0; % boundary conditions 18 | p.umax = 1; % maximum absolute force 19 | 20 | % direct transcription parameters 21 | % p.nt = 200; % number of node points 22 | p.nt = 100; % number of node points 23 | x0 = zeros(p.nt*(p.ns+p.nu)+1,1); % initial guess (all zeros) 24 | p.tfi = p.nt*(p.ns+p.nu)+1; 25 | x0(p.tfi) = 1; % final time guess 26 | p.tf = x0(p.tfi); 27 | p.t = linspace(p.t0,p.tf,p.nt)'; % time horizon 28 | p.h = diff(p.t); % step size 29 | 30 | % discretized variable indices in x = [y1,y2,u]; 31 | p.y1i = 1:p.nt; p.y2i = p.nt+1:2*p.nt; p.ui = 2*p.nt+1:3*p.nt; 32 | 33 | options = optimoptions(@fmincon,'display','iter','MaxFunEvals',1e5... 34 | ,'MaxIterations',1e2,'ConstraintTolerance',1.0000e-08); % options 35 | 36 | % solve the problem 37 | x = fmincon(@(x) objective(x,p),x0,[],[],[],[],[],[],@(x) constraints(x,p),options); 38 | 39 | % obtain the optimal solution 40 | y1 = x(p.y1i); y2 = x(p.y2i); u = x(p.ui); p.tf = x(p.tfi); % extract 41 | p.t = linspace(p.t0,p.tf,p.nt)'; 42 | soln.y1 = y1; soln.y2 = y2; soln.u = u; soln.tf = p.tf; soln.p = p; 43 | 44 | % plot 45 | showPlot(y1,y2,u,p) 46 | end 47 | 48 | % objective function 49 | function f = objective(x,p) 50 | f = x(p.tfi) - p.t0; % calculate objective i.e. minimise time 51 | end 52 | 53 | % constraint function 54 | function [c,ceq] = constraints(x,p) 55 | y1 = x(p.y1i); y2 = x(p.y2i); u = x(p.ui); % extract 56 | Y = [y1,y2]; F = [y2,u]; % create matrices (p.nt x p.ns) 57 | ceq1 = y1(1) - p.y10; % initial state conditions 58 | ceq2 = y2(1) - p.y20; 59 | ceq3 = y1(end) - p.y1f; % final state conditions 60 | ceq4 = y2(end) - p.y2f; 61 | 62 | % update step size 63 | p.tf = x(p.tfi); 64 | p.t = linspace(p.t0,p.tf,p.nt)'; % time horizon 65 | p.h = diff(p.t); % step size 66 | 67 | % integrate using trapezoidal quadrature 68 | ceq5 = Y(2:p.nt,1) - Y(1:p.nt-1,1) - p.h/2.*( F(1:p.nt-1,1) + F(2:p.nt,1) ); 69 | ceq6 = Y(2:p.nt,2) - Y(1:p.nt-1,2) - p.h/2.*( F(1:p.nt-1,2) + F(2:p.nt,2) ); 70 | c1 = x(p.tfi)-p.t0; 71 | c2 = u-p.umax; 72 | c3 = -u-p.umax; 73 | 74 | c = [c1;c2;c3]; ceq = [ceq1;ceq2;ceq3;ceq4;ceq5;ceq6]; % combine constraints 75 | end 76 | 77 | % plotting function 78 | function showPlot(y1,y2,u,p) 79 | close all 80 | 81 | t = tiledlayout(2,1); 82 | t.Padding = 'compact'; 83 | t.TileSpacing = 'compact'; 84 | % title(t, ['Optimal time taken is ', num2str(p.t(end)), ' secs.']); 85 | 86 | nexttile 87 | hold on 88 | grid on 89 | plot(p.t,y1,'b','LineWidth',1); 90 | plot(p.t,y2,'Color',[0, 0.5, 0],'LineWidth',1); 91 | legend('Pos','Vel'); 92 | xlabel("Time (in sec)"); 93 | xlim([0,p.t(end)]); 94 | hold off 95 | 96 | nexttile 97 | grid on 98 | stairs(p.t,u,'r--','LineWidth',1); 99 | legend('u'); 100 | ylabel("Acceleration"); 101 | xlabel("Time (in sec)"); 102 | xlim([0,p.t(end)]); 103 | ylim([-p.umax,p.umax]*1.2) 104 | 105 | % To print the figure 106 | % print('./results/optimal_sol_single_shooting','-dpng') 107 | 108 | end -------------------------------------------------------------------------------- /Optimal-Control-Problems/Double_Integrator/results/optimal_sol_collocation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nakulrandad/Direct-Methods-for-Optimal-Control/1ab440475578b478c0818ddcc8fb9b49e0931de6/Optimal-Control-Problems/Double_Integrator/results/optimal_sol_collocation.png -------------------------------------------------------------------------------- /Optimal-Control-Problems/Double_Integrator/results/optimal_sol_multiple_shooting.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nakulrandad/Direct-Methods-for-Optimal-Control/1ab440475578b478c0818ddcc8fb9b49e0931de6/Optimal-Control-Problems/Double_Integrator/results/optimal_sol_multiple_shooting.png -------------------------------------------------------------------------------- /Optimal-Control-Problems/Double_Integrator/results/optimal_sol_single_shooting.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/nakulrandad/Direct-Methods-for-Optimal-Control/1ab440475578b478c0818ddcc8fb9b49e0931de6/Optimal-Control-Problems/Double_Integrator/results/optimal_sol_single_shooting.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # [Direct Methods for Optimal Control](https://nakulrandad.github.io/Direct-Methods-for-Optimal-Control/) 2 | [![DOI](https://zenodo.org/badge/366012598.svg)](https://doi.org/10.5281/zenodo.4746128) 3 | 4 | ## Overview 5 | 6 | This repository includes [codes](Optimal-Control-Problems) and [documentation](Documentation) created as a part of my *Supervised Learning Project* (SLP). 7 | The project was guided by [Prof. Debasish Chatterjee](https://www.sc.iitb.ac.in/~chatterjee/master/homepage/index.html) from Systems and Control Engineering, Indian Institute of Technology Bombay. 8 | 9 | ## Abstract 10 | 11 | Here, we study the numerical methods for optimal control. In particular, 12 | the direct methods are discussed with an objective to showcase an approach for 13 | general optimal control problems. These methods, which include single shooting, 14 | multiple shooting and collocation methods, are relatively simple to follow and effectively 15 | solve a wide variety of optimization problems. We illustrate each of the 16 | methods by working through example problems. 17 | 18 | ## Contributors 19 | 20 | * [Nakul Randad](https://nakulrandad.github.io) 21 | 22 | ## Project Tree 23 | ``` 24 | . 25 | ├───Documentations 26 | └───Optimal-Control-Problems 27 | ├───Bryson_Denham 28 | │ └───results 29 | └───Double_Integrator 30 | └───results 31 | ``` 32 | --------------------------------------------------------------------------------