function setup = gpopsNlp2oc(setup);
%------------------------------------------------------------------%
% Convert the NLP solution to trajectory format %
%------------------------------------------------------------------%
% GPOPS Copyright (c) Anil V. Rao, Geoffrey T. Huntington, David %
% Benson, Michael Patterson, Christopher Darby, & Camila Francolin %
%------------------------------------------------------------------%
clear snoptcmex
ps = setup.ps;
result = setup.result;
variable_indices = setup.variable_indices;
constraint_indices = setup.constraint_indices;
variables = setup.variables;
constraints = setup.constraints;
sizes = setup.sizes;
nodes = setup.nodes;
numvars = setup.numvars;
numlin = setup.numlin;
numnonlin = setup.numnonlin;
numphases = setup.numphases;
numlinks = setup.numlinks;
x = result.x;
lamvars = result.xmul; % Get the lagrange multipliers for
% the variables
lamlin = result.Amul; % Get the lagrange multipliers for
% the linear constraints
lambda = result.Fmul(1:numnonlin-numlinks);
solution = cell(numphases,9); % Set up a cell array to store the solution
cost = 0;
varshift = 0;
conshift = 0;
for i=1:numphases;
%--------------------------------------------------------------------%
% First construct the primal solution to the optimal control %
% problem (i.e., states, controls, and parameters) %
%--------------------------------------------------------------------%
xcurr = x(variable_indices{i});
nstates = sizes(i,1);
ncontrols = sizes(i,2);
nparameters = sizes(i,3);
npaths = sizes(i,4);
nevents = sizes(i,5);
state_indices = 1:(nodes(i)+2)*nstates;
if ncontrols>0,
control_indices = state_indices(end)+1:state_indices(end)+nodes(i)*ncontrols;
t0_index = control_indices(end)+1;
else
control_indices = [];
t0_index = state_indices(end)+1;
end;
tf_index = t0_index+1;
parameter_indices = tf_index+1:tf_index+nparameters;
t0 = xcurr(t0_index);
tf = xcurr(tf_index);
state_vector = xcurr(state_indices);
control_vector = xcurr(control_indices);
tau_all = [-1; ps{i,2}; 1];
t_all = (tf-t0)*(tau_all+1)/2+t0;
t_gauss = t_all(2:end-1);
if nstates>0,
state_matrix = reshape(state_vector,nodes(i)+2,nstates);
state_gauss = state_matrix(2:end-1,:);
else
state_matrix = [];
end;
if ncontrols>0,
control_matrix = reshape(control_vector,nodes(i),ncontrols);
control_t0 = zeros(1,ncontrols);
control_tf = control_t0;
if ~isequal(t0,tf),
for j=1:ncontrols
control_t0(j) = interp1(t_gauss,control_matrix(:,j),t0,'spline','extrap');
control_tf(j) = interp1(t_gauss,control_matrix(:,j),tf,'spline','extrap');
end;
else
control_t0 = control_matrix(1,:);
control_tf = control_matrix(end,:);
end;
control_matrix = [control_t0; control_matrix; control_tf];
else
control_matrix = [];
end;
parameters = xcurr(parameter_indices);
%------------------------------%
% Next, construct the costates %
%------------------------------%
lamcurr = lambda(conshift+1:conshift+(nodes(i)+1)*nstates);
lamcurr = reshape(lamcurr,nodes(i)+1,nstates);
costatef = lamcurr(end,:);
costate_gauss = zeros(nodes(i),nstates);
Inverse_Weight_Matrix = diag(1./ps{i,3});
costate_gauss = Inverse_Weight_Matrix*lamcurr(1:end-1,:);
costatef_matrix = repmat(costatef,nodes(i),1);
DD = ps{i,1}(:,1);
DD = repmat(DD,1,nstates);
costate0 = sum(-DD.*lamcurr(1:end-1,:),1);
alpha0 = ps{i,3}*ps{i,1}(:,1);
costate0 = costate0+(1+alpha0)*costatef;
costates = [costate0; costate_gauss; costatef];
%--------------------------------------------%
% Solve for the initial and terminal control %
%--------------------------------------------%
if ncontrols>0,
ss0 = '---------------------------------------------------';
ss=strcat(['BEGIN: Computation of Endpoint Controls in Phase ',num2str(i)]);
disp(' ');
disp(' ');
disp(ss0);
disp(ss);
disp(ss0);
ulow = setup.limits{1}{i,3};
uupp = setup.limits{2}{i,3};
if npaths>0,
pathmin = setup.limits{1}{i,5};
pathmax = setup.limits{2}{i,5};
else
pathmin = [];
pathmax = [];
end;
Flow = [-Inf; pathmin];
Fupp = [ Inf; pathmax];
global extras funcs;
extras{1} = t_all(1);
extras{2} = state_matrix(1,:);
extras{3} = parameters;
extras{4} = costate0;
extras{5} = i;
funcs = setup.funcs;
usrfun = 'gpopsEndPointControl';
snscreen on
uguess = control_t0.';
snseti('Derivative Option',0);
snprint('snoptmain0.out');
snseti('Timing level',3);
% ---------------------------------------%
% Compute the Control at Initial Time %
% Using the Pontryagin Minimum Principle %
% ---------------------------------------%
m = length(Flow); n = length(uguess);
umul = zeros(n,1); ustate = zeros(n,1);
Fmul = zeros(m,1); Fstate = zeros(m,1);
ObjAdd = 0; ObjRow = 1; solveopt = 1;
Sjacobian = ones(m-1,n);
Alinear = [];
S_all = [ones(1,n); Sjacobian; zeros(size(Alinear))];
[iGfun,jGvar,SS]=find(S_all);
iGfun = iGfun(:);
jGvar = jGvar(:);
A = []; iAfun = []; jAvar = [];
[u0out,F0out,xmul0out,Fmul0out,inform0] = snoptcmex(solveopt,uguess,ulow,uupp,umul,ustate,Flow,Fupp,Fmul,Fstate,ObjAdd,ObjRow,A,iAfun,jAvar,iGfun,jGvar,usrfun);
extras{1} = t_all(end);
extras{2} = state_matrix(end,:);
extras{3} = parameters;
extras{4} = costatef;
extras{5} = i;
funcs = setup.funcs;
uguess = control_tf.';
snseti('Derivative Option',0);
snprint('snoptmainF.out');
snseti('Timing level',3);
% ---------------------------------------%
% Compute the Control at Initial Time %
% Using the Pontryagin Minimum Principle %
% ---------------------------------------%
m = length(Flow); n = length(uguess);
umul = zeros(n,1); ustate = zeros(n,1);
Fmul = zeros(m,1); Fstate = zeros(m,1);
ObjAdd = 0; ObjRow = 1; solveopt = 1;
[uFout,FFout,umulFout,FmulFout,informF] = snoptcmex(solveopt,uguess,ulow,uupp,umul,ustate,Flow,Fupp,Fmul,Fstate,ObjAdd,ObjRow,A,iAfun,jAvar,iGfun,jGvar,usrfun);
ss0 = '------------------------------------------------';
ss=strcat(['END: Computation of Endpoint Controls in Phase ',num2str(i)]);
disp(' ');
disp(' ');
disp(ss0);
disp(ss);
disp(ss0);
control_matrix(1,:) = u0out;
control_matrix(end,:) = uFout;
else
if npaths>0,
pathmin = setup.limits{1}{i,5};
pathmax = setup.limits{2}{i,5};
else
pathmin = [];
pathmax = [];
end;
Fmul0out = [0; zeros(size(pathmax))];
FmulFout = [0; zeros(size(pathmax))];
end;
if npaths>0,
pathmults = [];
pathmults = lambda(conshift+(nodes(i)+1)*nstates+1:conshift+(nodes(i)+1)*nstates+npaths*nodes(i));
pathmults = reshape(pathmults,nodes(i),npaths);
for j=1:npaths
pathmults(:,j) = 2*pathmults(:,j)/(tf-t0)./setup.ps{i,3}.';
end;
if ncontrols>0,
pathmults = [Fmul0out(2:end).'; pathmults; FmulFout(2:end).'];
else
pathmults0 = zeros(1,npaths);
pathmultsF = zeros(1,npaths);
for k=1:npaths
pathmult0(k) = interp1(t_all(2:end-1),pathmults(:,k),t_all(1),'spline','extrap');
pathmultF(k) = interp1(t_all(2:end-1),pathmults(:,k),t_all(end),'spline','extrap');
end;
end;
else
pathmults = [];
end;
% -------------------------------------------------------------------%
% Put all of the information from the primal and dual solutions %
% into the appropriate location in the cell array SOLUTION %
% i = phase number %
% solution{i,1}: vector containing time %
% solution{i,2}: N+2 by nx matrix containing states %
% solution{i,3}: N+2 by nu matrix containing controls %
% solution{i,4}: vector of length q containing static parameters %
% solution{i,5}: N+2 by np matrix containing path multipliers %
% solution{i,6}: N+2 by np matrix containing costates %
% solution{i,7}: vector of length N+2 containing Hamiltonian %
% solution{i,8}: scalar containing the Mayer cost %
% solution{i,9}: scalar containing the Lagrange cost %
solphase{1,1} = t_all(1);
solphase{1,2} = state_matrix(1,:).';
solphase{1,3} = t_all(end);
solphase{1,4} = state_matrix(end,:).';
solphase{2,1} = t_all;
solphase{2,2} = state_matrix;
solphase{2,3} = control_matrix;
solphase{2,4} = parameters;
[Mayer,Lagrange] = feval(setup.funcs{1},solphase,i);
soldae{1,1} = t_all;
soldae{1,2} = state_matrix;
soldae{1,3} = control_matrix;
soldae{1,4} = parameters;
dae = feval(setup.funcs{2},soldae,i);
Hamiltonian = zeros(nodes(i)+2,1);
Hamiltonian = Lagrange+sum(costates.*dae(:,1:nstates),2);
varshift = varshift+length(variable_indices{1,i});
conshift = conshift+length(constraint_indices{1,i});
Lagrange_Cost = (tf-t0)*setup.ps{i,3}*Lagrange(2:end-1)/2;
cost = cost + Mayer + Lagrange_Cost;
solution(i,:) = {t_all,state_matrix,control_matrix,parameters,costates,pathmults,Hamiltonian,Mayer,Lagrange_Cost};
end;
solution(end+1,:) = {'time','states','controls','parameters','costates','path_multipliers','Hamiltonian','Mayer_cost','Lagrange_cost'};
setup.solution = solution;
setup.cost = cost;
