runOPT.asv

function [result, allresults] = runOPT(NSU)

% Declare parameters
params.m = 5;           %weight
params.l = 1.2;         %length
params.g = 9.81;        %gravity
% params.ktheta = 0.01;
% params.kthetadot = 0.01;
params.T = 10;          %Duration of motion

params.nstates  = 2;    %Number of states in the dynamic system
params.ncontrols= 1;    %Number of controls

params.NperSU = 60;     %Number of collocation nodes
params.NSU = 1;         %Start with one swingup

params.solver = 'IPOPT'; %Solved using IPOPT

params = getParams(params);                     %Do some bookkeeping on number of constraints and optimization variables
params.omega = zeros(params.nstates,params.N);  %This is the noise vector, initially zeros
params.targetangle = pi/2;                      %The

[X0, L, U] = getIniConBound(params);

[~, params] = conjacstructure(L, U, params);

%First result witout noise for desired trajectory
result1 = Optimize(X0, L, U, params);

% Now add more swingups and noise
params.NSU = NSU;
params.ineq = 0;
params = getParams(params);
params.omega = 0.001*randn(params.nstates,params.N); %Added noise, low pass filtered

[X0, L, U] = getIniConBound(params, result1);
% X0(end-1:end) = -1000;
[~, params] = conjacstructure(L, U, params);
result2 = Optimize(X0, L, U, params);

A = [0 1; -result2.params.g/result2.params.l+result2.X(end-1)/(result2.params.m*result2.params.l^2) result2.X(end)/(result2.params.m*result2.params.l^2)];
eig_A = eig(A);

c = confun1(result2.X, result2.params);
if and(result2.info == 2, max(c([1:end-3 end-1:end])) < 1e-4)
    if ~and(eig_A(1)<0,eig_A(2)<0)
        result2.info = 100;
    end
end

% More noise
X0 = result2.X;
params.omega = 0.01*randn(params.nstates,params.N); %Added noise, low pass filtered
[~, params] = conjacstructure(L, U, params);
result3 = Optimize(X0, L, U, params);

X0 = result3.X;
params.omega = 0.05*randn(params.nstates,params.N); %Added noise, low pass filtered
[~, params] = conjacstructure(L, U, params);
result4 = Optimize(X0, L, U, params);

X0 = result4.X;
params.omega = 0.1*randn(params.nstates,params.N); %Added noise, low pass filtered
[~, params] = conjacstructure(L, U, params);
result5 = Optimize(X0, L, U, params);

X0 = result5.X;
params.omega = 0.5*randn(params.nstates,params.N); %Added noise, low pass filtered
[~, params] = conjacstructure(L, U, params);
result6 = Optimize(X0, L, U, params);

X0 = result6.X;
params.omega = 1*randn(params.nstates,params.N); %Added noise, low pass filtered
[~, params] = conjacstructure(L, U, params);
result = Optimize(X0, L, U, params);

A = [0 1; -result.params.g/result.params.l+result.X(end-1)/(result.params.m*result.params.l^2) result.X(end)/(result.params.m*result.params.l^2)];
eig_A = eig(A);

c = confun1(result.X, result.params);
if and(result.info == 2, max(c([1:end-3 end-1:end])) < 1e-4)
    if ~and(eig_A(1)<0,eig_A(2)<0)
        result.info = 100;
    end
end

allresults(1) = result1;
allresults(2) = result2;
allresults(3) = result3;
allresults(4) = result4;
allresults(5) = result5;
allresults(6) = result6;
allresults(7) = result;