All files added

Author: nakulrandad

Documentations/Direct-Methods-SLP-Presentation.pdf

@@ -0,0 +1,161 @@
+%--------------------------------------------------------------------------
+% Bryson_Denham_collocation.m
+% This file solves the Bryson-Denham problem by
+% direct collocation method using Casadi
+%--------------------------------------------------------------------------
+% Primary Contributor: Nakul Randad, Indian Institute of Technology Bombay
+%--------------------------------------------------------------------------
+% CasADi v3.5.5
+import casadi.*
+
+close all
+% clc
+
+%% Set up collocation points 
+% Degree of interpolating polynomial
+d = 3;
+
+% Get collocation points
+tau = collocation_points(d, 'legendre');
+
+% Collocation linear maps
+[C,D,B] = collocation_coeff(tau);
+
+%% Declare decision variables
+T = 1;             % Time horizon
+limit = 1/9;       % position limit
+
+% Declare model variables
+x1 = SX.sym('x1'); % position
+x2 = SX.sym('x2'); % velocity
+x = [x1; x2];
+u = SX.sym('u');
+
+%% Dynamics and Objective
+% Model equations
+xdot = [x2; u];
+
+% Objective term
+L = u^2/2;
+
+% Continuous time dynamics
+f = Function('f', {x, u}, {xdot, L});
+
+%% Control discretization
+N = 100; % number of control intervals
+h = T/N;
+
+%% Set up solver
+% Start with an empty NLP
+
+opti = Opti();
+J = 0;
+
+% Initial conditions
+Xk = opti.variable(2);
+opti.subject_to(Xk==[0; 1]);
+opti.set_initial(Xk, [0; 1]);
+
+% Collect all states/controls
+Xs = {Xk};
+Us = {};
+cost = [];
+
+% Formulate the NLP
+for k=0:N-1
+   % New NLP variable for the control
+   Uk = opti.variable();
+   Us{end+1} = Uk;
+   opti.set_initial(Uk, 0);
+
+   % Decision variables for helper states at each collocation point
+   Xc = opti.variable(2, d);
+   opti.subject_to(Xc(1,:) <= limit); % inequality constraint on state
+   opti.set_initial(Xc, repmat([0;0],1,d));
+
+   % Evaluate ODE right-hand-side at all helper states
+   [ode, quad] = f(Xc, Uk);
+
+   % Add contribution to quadrature function
+   int_L = quad*B*h;
+   J = J + int_L;
+   cost = [cost; J];
+
+   % Get interpolating points of collocation polynomial
+   Z = [Xk Xc];
+
+   % Get slope of interpolating polynomial (normalized)
+   Pidot = Z*C;
+   % Match with ODE right-hand-side 
+   opti.subject_to(Pidot == h*ode);
+
+   % State at end of collocation interval
+   Xk_end = Z*D;
+
+   % New decision variable for state at end of interval
+   Xk = opti.variable(2);
+   Xs{end+1} = Xk;
+   opti.subject_to(Xk(1) <= limit);% inequality constraint on state
+   opti.set_initial(Xk, [0;0]);
+
+   % Continuity constraints
+   opti.subject_to(Xk_end==Xk)
+end
+
+% Boundary conditions
+opti.subject_to(Xs{end}(1)==0);
+opti.subject_to(Xs{end}(2)==-1);
+
+%% Optimisation solver
+
+Xs = [Xs{:}];
+Us = [Us{:}];
+
+opti.minimize(J); % minimise the objective function
+
+opti.solver('ipopt'); % backend NLP solver
+
+tic
+sol = opti.solve(); % Solve actual problem
+toc
+
+x_opt = sol.value(Xs);
+u_opt = sol.value(Us);
+cost_opt = sol.value(cost);
+
+%% Post-processing
+time = linspace(0, T, N+1);
+t = tiledlayout(2,2);
+t.Padding = 'compact';
+t.TileSpacing = 'compact';
+
+nexttile
+hold on
+plot(time,x_opt(1,:),'b','LineWidth',1);
+yline(limit,'k--','LineWidth',1);
+hold off
+ylim([-inf, limit*1.05])
+ylabel('Position');
+xlabel('Time [s]');
+legend('x','$x < \frac{1}{9}$','Interpreter','latex','Location', 'South');
+
+nexttile
+plot(time, x_opt(2,:),'Color',[0, 0.5, 0],'LineWidth',1);
+ylabel('Speed');
+xlabel('Time [s]');
+legend('v','Location', 'South')
+
+nexttile
+plot(time, [u_opt nan],'r','LineWidth',1);
+xlabel('Time [s]');
+ylabel('Thrust');
+legend('u','Location', 'South');
+
+nexttile
+plot(time,[cost_opt; nan],'--','LineWidth',1);
+xlabel('Time [s]');
+ylabel('Objective');
+legend('$\frac{1}{2} \int u^2$','Interpreter','latex','Location', 'South');
+
+% To print the figure
+% print('./results/optimal_sol_collocation','-dpng')

Documentations/Direct-Methods-SLP-Report.pdf

@@ -0,0 +1,105 @@
+%--------------------------------------------------------------------------
+% Bryson_Denham_multiple_shooting.m
+% This file solves the Bryson-Denham problem by
+% multiple-shooting method using Casadi
+%--------------------------------------------------------------------------
+% Primary Contributor: Nakul Randad, Indian Institute of Technology Bombay
+%--------------------------------------------------------------------------
+% CasADi v3.5.5
+import casadi.*
+
+close all
+% clc
+
+%% Setting up the problem
+N = 100; % number of control intervals
+opti = casadi.Opti(); % Optimization problem
+
+%% Declare decision variables
+X = opti.variable(2,N+1); % state trajectory
+pos = X(1,:);
+speed = X(2,:);
+U = opti.variable(1,N);   % control trajectory (throttle)
+T = 1;        % final time
+limit = 1/9;     % position limit
+time = linspace(0, T, N+1)'; % time horizon
+
+%% Set up the objective
+L = U.^2/2; % integrand
+dt = T/N; % length of a control interval
+cost = [];
+obj = 0;
+for i = 1:N
+    obj = obj + L(i)*dt;
+    cost = [cost;obj];
+end
+opti.minimize(obj); % minimize objective
+
+%% System dynamics
+f = @(x,u) [x(2);u]; % dx/dt = f(x,u)
+
+%% Numerical integration and constraint to make zero gap
+for k=1:N % loop over control intervals
+   % Runge-Kutta 4 integration
+   k1 = f(X(:,k),         U(:,k));
+   k2 = f(X(:,k)+dt/2*k1, U(:,k));
+   k3 = f(X(:,k)+dt/2*k2, U(:,k));
+   k4 = f(X(:,k)+dt*k3,   U(:,k));
+   x_next = X(:,k) + dt/6*(k1+2*k2+2*k3+k4);
+   opti.subject_to(X(:,k+1)==x_next); % close the gaps
+end
+
+%% Path constraints
+opti.subject_to(pos<=limit);           % position limit
+
+%% Boundary conditions
+opti.subject_to(pos(1)==0);   % start at position 0
+opti.subject_to(speed(1)==1); 
+opti.subject_to(pos(N+1)==0); % finish line at position 0
+opti.subject_to(speed(N+1)==-1);
+
+%% Initial guess
+opti.set_initial(speed, 1);
+opti.set_initial(pos, 0);
+
+%% Solver set up
+opti.solver('ipopt'); % set numerical backend
+tic
+sol = opti.solve();   % actual solve
+toc
+
+%% Post-processing
+t = tiledlayout(2,2);
+t.Padding = 'compact';
+t.TileSpacing = 'compact';
+
+nexttile
+hold on
+plot(time,sol.value(pos),'b','LineWidth',1);
+yline(limit,'k--','LineWidth',1);
+hold off
+ylim([-inf, limit*1.05])
+ylabel('Position');
+xlabel('Time [s]');
+legend('x','$x < \frac{1}{9}$','Interpreter','latex','Location', 'South');
+
+nexttile
+plot(time,sol.value(speed),'Color',[0, 0.5, 0],'LineWidth',1);
+ylabel('Speed');
+xlabel('Time [s]');
+legend('v','Location', 'South')
+
+nexttile
+stairs(time(1:end-1),sol.value(U),'r','LineWidth',1);
+xlabel('Time [s]');
+ylabel('Thrust');
+legend('u','Location', 'South');
+
+nexttile
+plot(time(1:end-1),sol.value(cost),'--','LineWidth',1);
+xlabel('Time [s]');
+ylabel('Objective');
+legend('$\frac{1}{2} \int u^2$','Interpreter','latex','Location', 'South');
+
+% To print the figure
+% print('./results/optimal_sol_multiple_shooting','-dpng')

Optimal-Control-Problems/Bryson_Denham/Bryson_Denham_collocation.m

@@ -0,0 +1,105 @@
+%--------------------------------------------------------------------------
+% Bryson_Denham_single_shooting.m
+% This file solves the Bryson-Denham problem by single-shooting method
+% using fmincon
+% (employs the trapezodial rule with composite trapezoidal quadrature)
+%--------------------------------------------------------------------------
+% Primary Contributor: Nakul Randad, Indian Institute of Technology Bombay
+%--------------------------------------------------------------------------
+
+soln = method();
+
+% actual implementation
+function soln = method
+    % problem parameters
+    p.ns = 2; p.nu = 1; % number of states and controls
+    p.t0 = 0; p.tf = 1; % time horizon
+    p.y10 = 0; p.y1f = 0; p.y20 = 1; p.y2f = -1; % boundary conditions
+    p.l = 1/9;
+    
+    % direct transcription parameters
+%     p.nt = 20; % number of node points
+    p.nt = 100; % number of node points
+    p.t = linspace(p.t0,p.tf,p.nt)'; % time horizon
+    p.h = diff(p.t); % step size
+    
+    % discretized variable indices in x = [y1,y2,u];
+    p.y1i = 1:p.nt; p.y2i = p.nt+1:2*p.nt; p.ui = 2*p.nt+1:3*p.nt;
+    x0 = zeros(p.nt*(p.ns+p.nu),1); % initial guess (all zeros)
+    options = optimoptions(@fmincon,'display','iter','MaxFunEvals',1e5); % options
+    
+    % solve the problem
+    tic
+    x = fmincon(@(x) objective(x,p),x0,[],[],[],[],[],[],@(x) constraints(x,p),options);
+    toc
+    
+    % obtain the optimal solution
+    y1 = x(p.y1i); y2 = x(p.y2i); u = x(p.ui); % extract
+    soln.y1 = y1; soln.y2 = y2; soln.u = u; soln.p = p;
+    
+    % plots
+    showPlot(y1,y2,u,p)
+end
+
+% objective function
+function f = objective(x,p)
+    u = x(p.ui); % extract
+    L = u.^2/2; % integrand
+    f = trapz(p.t,L); % calculate objective
+end
+
+% constraint function
+function [c,ceq] = constraints(x,p)
+    y1 = x(p.y1i); y2 = x(p.y2i); u = x(p.ui); % extract
+    Y = [y1,y2]; F = [y2,u]; % create matrices (p.nt x p.ns)
+    ceq1 = y1(1) - p.y10; % initial state conditions
+    ceq2 = y2(1) - p.y20;
+    ceq3 = y1(end) - p.y1f; % final state conditions
+    ceq4 = y2(end) - p.y2f;
+    
+    % integrate using trapezoidal quadrature
+    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) );
+    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) );
+    c1 = y1 - p.l; % path constraints
+    
+    c = c1; ceq = [ceq1;ceq2;ceq3;ceq4;ceq5;ceq6]; % combine constraints
+end
+
+% plotting function
+function showPlot(y1,y2,u,p)
+    close all
+    t = tiledlayout(2,2);
+    t.Padding = 'compact';
+    t.TileSpacing = 'compact';
+    
+    nexttile
+    hold on
+    plot(p.t,y1,'b','LineWidth',1);
+    yline(p.l,'k--','LineWidth',1);
+    scatter(p.t0,p.y10,[],'g','filled');
+    scatter(p.tf,p.y1f,[],'r','filled');
+    hold off
+    ylim([-inf, p.l*1.05])
+    ylabel('Position');
+    xlabel('Time [s]');
+    legend('$y1$','$y1 < \frac{1}{9}$',"$y1_0$","$y1_f$",'Interpreter','latex','Location', 'South');    
+    
+    nexttile
+    hold on
+    plot(p.t,y2,'Color',[0, 0.5, 0],'LineWidth',1);
+    scatter(p.t0,p.y20,[],'g','filled');
+    scatter(p.tf,p.y2f,[],'r','filled');
+    hold off
+    ylabel('Speed');
+    xlabel('Time [s]');
+    legend('$y2$',"$y2_0$","$y2_f$",'Interpreter','latex','Location', 'South')
+
+    nexttile(3,[1,2])
+    stairs(p.t,u,'r','LineWidth',1);
+    xlabel('Time [s]');
+    ylabel('Thrust');
+    legend('u','Location', 'South');
+
+    % To print the figure
+%     print('./results/optimal_sol_single_shooting','-dpng')
+end