function output = gpops(setup);
% ---------------------------------------------------------------- %
% GPOPS: Gauss Pseudopsectral Optimal Control Program %
% ---------------------------------------------------------------- %
% GPOPS is a MATLAB(R) program for solving non-sequential %
% multiple-phase optimal control problems. GPOPS uses the Gauss %
% pseudospectral method (GPM) where orthogonal collocation is %
% performed at the Legendre-Gauss points. GPOPS is based %
% entirely on mathematical theory that has been published in the %
% open literature. Specifically, a good portion of the theory %
% of the Gauss pseudospectral method can be found in the %
% publications: %
% %
% [1] Benson, D. A., Huntington, G. T., Thorvaldsen, T. P., and %
% Rao, A. V., "Direct Trajectory Optimization and Costate %
% Estimation via an Orthogonal Collocation Method," Journal %
% of Guidance, Control, and Dynamics, Vol. 29, No. 6, %
% November-December 2006, pp. 1435-1440. %
% %
% [2] Benson, D. A., "A Gauss Pseudospectral Transcription for %
% Optimal Control," Ph.D. Thesis, Dept. of Aeronautics and %
% Astronautics, Massachusetts Institute of Technology, %
% February 2005. %
% %
% [3] Huntington, G. T., "Advancement and Analysis of a Gauss %
% Pseudospectral Transcription for Optimal Control," %
% Ph.D. Thesis, Dept. of Aeronautics and Astronautics, $
% Massachusetts Institute of Technology, May 2007. %
% %
% [4] Rao, A. V., Benson, D. A., Huntington, G. T., Francolin, C. %
% Darby, C. L., and Patterson, M. A., "User's Manual for %
% GPOPS: A MATLAB Package for Dynamic Optimization Using the %
% Gauss Pseudospectral Method, University of Florida, Report, %
% August 2008. %
% %
% Further information about the pseudoespectral theory used in %
% GPOPS and the applications of this theory to various problems %
% of interest can be found at Anil V. Rao's website at %
% http://fdol.mae.ufl.edu. %
% %
%------------------------------------------------------------------%
% Authors of the GPOPS Code: %
% David A. Benson: Draper Laboratory, Cambridge, MA %
% Geoffrey Huntington: Blue Origin, LLC, Seattle, WA %
% Michael Patterson: University of Florida, Gainesville, FL %
% Christopher Darby: University of Florida, Gainesville, FL %
% Camila Francolin: University of Florida, Gainesville, FL %
% Anil V. Rao: University of Florida, Gainesville, FL %
% File creation date: 5 August 2008 %
%------------------------------------------------------------------%
% GPOPS Copyright (c) Anil V. Rao, Geoffrey T. Huntington, David %
% Benson, Michael Patterson, Christopher Darby, & Camila Francolin %
%------------------------------------------------------------------%
% For Licencing Information, Please See File LICENSE %
% ---------------------------------------------------------------- %
% %
% See GPOPS Manual for usage %
% %
% ---------------------------------------------------------------- %
% Clear any previous calls to the SNOPT mex file %
%------------------------------------------------------------------%
clear snoptcmex;
% ---------------------------------------------------------------- %
% SNOPTA does not allow for variable input arguments. %
% Consequently, a copy needs to be make of the master structure %
% SETUP. This structure needs to be made a global variable for use %
% the various functions. %
% ---------------------------------------------------------------- %
global mysetup;
% --------------------------------------------------------------- %
% Get the sizes in each phase of the optimal control problem %
% --------------------------------------------------------------- %
setup = gpopsGetSizes(setup);
% --------------------------------------------------------------- %
% Get the bounds in each phase of the optimal control problem %
% --------------------------------------------------------------- %
setup = gpopsGetBounds(setup);
% -------------------------------------------------- %
% Print a description of the optimal control problem %
% -------------------------------------------------- %
gpopsPrint(setup);
% --------------------------------- %
% Get the initial guess for the NLP %
% --------------------------------- %
setup = gpopsGetGuess(setup);
% --------------------------- %
% Scale the nonlinear program %
% --------------------------- %
setup = gpopsScaleNlp(setup);
% ----------------------------------------- %
% Determine the sparsity pattern of the NLP %
% ----------------------------------------- %
setup = gpopsSparsity(setup);
% ------------------------------------------- %
% Lower and upper bounds on the NLP variables %
% ------------------------------------------- %
xlow = setup.varbounds_min;
xupp = setup.varbounds_max;
% --------------------------------------------------- %
% Lower and upper bounds on the nonlinear constraints %
% --------------------------------------------------- %
clow = setup.conbounds_min;
cupp = setup.conbounds_max;
% ------------------------- %
% Initial guess for the NLP %
% ------------------------- %
init = setup.init_vector;
% -------------------------------------------------------- %
% Matrix containing coefficients of the linear constraints %
% -------------------------------------------------------- %
Alinear = setup.Alinear;
% ------------------------------------------------ %
% Lower and upper bounds on the linear constraints %
% ------------------------------------------------ %
Alinmin = setup.Alinmin;
Alinmax = setup.Alinmax;
% -------------------------------------------------------------- %
% Modify the lower & upper bounds on the variables & constraints %
% depending upon whether or not the user has chosen to %
% automatically scale the NLP. %
%--------------------------------------------------------------- %
if isfield(setup,'autoscale'),
if isequal(setup.autoscale,'on'),
xlow = xlow.*setup.column_scales;
xupp = xupp.*setup.column_scales;
clow = clow.*setup.row_scales;
cupp = cupp.*setup.row_scales;
init = init.*setup.column_scales;
Alinear = Alinear*diag(1./setup.column_scales);
disp(' ')
disp('Automatic Scaling Turned On');
% ------------------------------------------------- %
% Give Warning for infinite bounds with autoscaling %
% ------------------------------------------------- %
if ~isempty(find(isinf(xlow)|isinf(xupp),1))
disp('WARNING!!! Automatic Scaling may not work with infinite bounds')
disp(' ')
end
elseif isequal(setup.autoscale,'off'),
setup.column_scales = ones(size(setup.column_scales));
setup.row_scales = ones(size(setup.row_scales));
disp(' ')
disp('Automatic Scaling Turned Off');
else
error('setup.autoscale not set correctly');
end;
else
setup.column_scales = ones(size(setup.column_scales));
setup.row_scales = ones(size(setup.row_scales));
end;
Alinear = sparse(Alinear);
setup.Alinear = Alinear;
S = setup.sparsity_nonlinear;
% ---------------------------------------------------------------- %
% Setup diagonal matrices containing the scale factors computed by %
% the automatic scaling routine. %
% ---------------------------------------------------------------- %
setup.Dx = diag(setup.column_scales);
setup.invDx = sparse(diag(1./setup.column_scales));
setup.DF = sparse(diag(setup.row_scales));
setup.invDF = sparse(diag(1./setup.row_scales));
% ---------------------------------------------------------- %
% SJACOBIAN is the sparsity pattern for the NON-CONSTANT %
% DERIVATIVES and DOES NOT include the CONSTANT derivatives. %
% ---------------------------------------------------------- %
Sjacobian = setup.sparsity_jac;
S_all = [ones(1,size(S,2)); Sjacobian; zeros(size(Alinear))];
% ---------------------------------------- %
% Find the row and column indices in S_ALL %
% ---------------------------------------- %
[iGfun,jGvar,SS]=find(S_all);
% ---------------------------------------- %
% Sort the row and column indices by rows. %
% ---------------------------------------- %
JJ = sortrows([iGfun jGvar SS],1);
iGfun = JJ(:,1);
jGvar = JJ(:,2);
setup.iGfun = iGfun;
setup.jGvar = jGvar;
% free memory
clear S SS JJ Sjacobian
% -------------------------------------------- %
% SCONSTANT constains the CONSTANT derivatives %
% -------------------------------------------- %
Sconstant = setup.sparsity_constant*setup.invDx;
Sconstant = setup.DF*Sconstant;
% ---------------------------------------------------------------- %
% ALINEAR_AUGMENTED is a matrix of the linear constraints plus the %
% constant derivatives. %
% ---------------------------------------------------------------- %
Alinear_augmented = [zeros(1,setup.numvars); Sconstant; Alinear];
[iAfun,jAvar,AA] = find(Alinear_augmented);
% ---------------------------------------- %
% Sort the row and column indices by rows. %
% ---------------------------------------- %
II = sortrows([iAfun jAvar AA],1);
iAfun = II(:,1);
jAvar = II(:,2);
AA = II(:,3);
% free memory
clear II Sconstant Alinear
% --------------------------------------------------------------- %
% This section checks to see if MATLAB automatic differentiation %
% is installed. If so, then it initializes the all global %
% variables associated with MAD and sets the appropriate %
% differentiation method for use with the NLP solver. %
% --------------------------------------------------------------- %
if isfield(setup,'derivatives'),
if isequal(setup.derivatives,'automatic'),
if ~checkMAD(0),
madinitglobals;
end;
% ------------------------------------------------- %
% Set up a bunch of stuff related to MAD when using %
% automatic differentiation %
% ------------------------------------------------- %
color_groups = MADcolor(S_all);
seed = MADgetseed(S_all,color_groups);
setup.seed = sparse(seed);
setup.sparsity_all = S_all;
setup.color_groups = color_groups;
% --------------------------------------------------------- %
% When using AUTOMATIC DIFFERENTIATION, SNOPT will call the %
% function 'gpopsuserfunAD.m" %
% --------------------------------------------------------- %
userfun = 'gpopsuserfunAD';
snseti ('Verify Level',-1);
snseti('Derivative Option',1);
xmadinit = fmad(init,speye(length(init)));
disp('Objective Gradient Being Estimated via Automatic Differentiation');
disp('Constraint Jacobian Being Estimated via Automatic Differentiation');
elseif isequal(setup.derivatives,'numerical'),
snseti('Derivative Option',0);
% --------------------------------------------------------- %
% When using NUMERICAL DIFFERENTIATION, SNOPT will call the %
% function 'gpopsuserfunFD.m" %
% --------------------------------------------------------- %
userfun = 'gpopsuserfunFD';
xmadinit = init;
disp('Objective Gradient Being Estimated via Finite Differencing');
disp('Constraint Jacobian Being Estimated via Finite Differencing');
elseif isequal(setup.derivatives,'complex'),
% --------------------------------------------------------- %
% When using COMPLEX-STEP DIFFERENTIATION, SNOPT will call %
% the function 'gpopsuserfunCS.m" %
% --------------------------------------------------------- %
userfun = 'gpopsuserfunCS';
xmadinit = init;
setup.hpert = 1e-20;
setup.deltaxmat = sqrt(-1)*setup.hpert*speye(setup.numvars);
setup.Jaczeros = zeros(setup.numnonlin+setup.numlin+1,setup.numvars);
disp('Objective Gradient Being Estimated via Complex Differentiation');
disp('Constraint Jacobian Being Estimated via Complex Differentiation');
else
error(['Unknown derivative option "%s" in setup.derivatives\n',...
'Valid options are:\n\tautomatic \n\tnumerical \n\tcomplex\n'],...
setup.derivatives);
end;
else
% --------------------------------------------------------- %
% When using NUMERICAL DIFFERENTIATION, SNOPT will call the %
% function 'gpopsuserfunFD.m" %
% --------------------------------------------------------- %
userfun = 'gpopsuserfunFD';
snseti('Derivative Option',0);
xmadinit = init;
disp('Objective Gradient Being Computed via Finite Differencing');
disp('Constraint Jacobian Being Computed via Finite Differencing');
end;
% ------------------------------------- %
% NUMLIN = Number of linear constraints %
% ------------------------------------- %
numlin = setup.numlin;
% ---------------------------------------- %
% NUMNONLIN = Number of linear constraints %
% ---------------------------------------- %
numnonlin = setup.numnonlin;
% ------------------------------------------------------------- %
% Pre-allocate a bunch of vectors for use in the user function. %
% This pre-allocation is done to reduce execution time. %
% ------------------------------------------------------------- %
% setup.initfuns = zeros(numnonlin+numlin+1,size(xmadinit,2));
% setup.initcons = zeros(numnonlin,size(xmadinit,2));
setup.initlincons = zeros(numlin,size(xmadinit,2));
% for j=1:setup.numphases;
% setup.initoderhs{j} = zeros(setup.nodes(j),size(xmadinit,2));
% setup.initfuns = zeros(numnonlin+numlin+1,size(xmadinit,2));
% setup.initcons = zeros(numnonlin,size(xmadinit,2));
% setup.initlincons = zeros(numlin,size(xmadinit,2));
% for j=1:setup.numphases;
% setup.initoderhs{j} = zeros(setup.nodes(j),size(xmadinit,2));
% setup.initoderhs{j} = repmat(setup.initoderhs{j},1,setup.sizes(j,1));
% end;
% setup.initfuns = zeros(numnonlin+numlin+1,size(xmadinit,2));
% setup.initcons = zeros(numnonlin,size(xmadinit,2));
% setup.initlincons = zeros(numlin,size(xmadinit,2));
% for j=1:setup.numphases;
% setup.initoderhs{j} = zeros(setup.nodes(j),size(xmadinit,2));
% setup.initoderhs{j} = repmat(setup.initoderhs{j},1,setup.sizes(j,1));
% end;
% end;
% -------------------------------------------------------- %
% Set up settings that are used by SNOPT for every problem %
% -------------------------------------------------------- %
snprint('snoptmain.out'); % Name of SNOPT Print File
snseti('Timing level',3); % Print Execution Time to File
snset('Hessian Limited Memory'); % Choose Hessian Type
snset('LU Complete Pivoting'); % Choose Pivoting Method
snseti('Verify Level',-1); % Derivative Verification Level
snseti('Iteration Limit',100000); % Iteration Limit
snseti('Major Iterations Limit',100000); % Major Iteration Limit
snseti('Minor Iterations Limit',100000); % Minor Iteration Limit
if isfield(setup,'tolerances');
if isequal(length(setup.tolerances),2),
if ~isempty(setup.tolerances{1}),
snsetr('Optimality Tolerance',setup.tolerances{1});
snsetr('Feasibility Tolerance',setup.tolerances{2});
else
snsetr('Optimality Tolerance',setup.tolerances{2});
end;
elseif isequal(length(setup.tolerances),1),
snsetr('Optimality Tolerance',setup.tolerances{1});
end;
end;
% --------------------------------------------------------------- %
% Set the lower and upper limits on the constraints and objective %
% function. The following assumptions are made: %
% Row 1 is the objective row %
% Rows 2 through numnonlin+1 are the nonlinear constraints %
% Rows numnonlin+2 to the end are the linear constraints %
% --------------------------------------------------------------- %
Flow = [-Inf; clow; Alinmin];
Fupp = [ Inf; cupp; Alinmax];
% -------------------------------------------------------------- %
% Initialize the Lagrange multipliers on the constraints to zero %
% -------------------------------------------------------------- %
Fmul = zeros(numnonlin+numlin+1,1);
Fstate = Fmul;
% ------------------------------------------------------------ %
% Initialize the Lagrange multipliers on the variables to zero %
% ------------------------------------------------------------ %
xmul = zeros(setup.numvars,1);
xstate = xmul;
% -------------------------------------------------- %
% Objrow =1 <=======> First row is the objective row %
% -------------------------------------------------- %
ObjRow = 1;
% -------------------------------------------------------------- %
% ObjAdd =0 <====> Do not add anything to the objective function %
% -------------------------------------------------------------- %
ObjAdd = 0;
% --------------------- %
% Turn on screen output %
% --------------------- %
snscreen on
% -------------------------------------------------------------- %
% Set MYSETUP equal to SETUP for global use in the optimization. %
% -------------------------------------------------------------- %
mysetup = setup;
% ------------------------- %
% Solve the NLP using SNOPT %
% ------------------------- %
[x,F,xmul,Fmul,info,xstate,Fstate,ns,ninf,sinf,mincw,miniw,minrw] = snsolve(init,xlow,xupp,xmul,xstate,Flow,Fupp,Fmul,Fstate,ObjAdd,ObjRow,AA,iAfun,jAvar,iGfun,jGvar,userfun);
snprint off;
% ------------------------------------ %
% Unscale the NLP (Decision) Variables %
% ------------------------------------ %
result.x = x./setup.column_scales;
% ---------------------------------------- %
% Unscale the multipliers on the variables %
% ---------------------------------------- %
result.xmul = setup.Dx*xmul;
% ------------------------------------------ %
% Unscale the multipliers on the constraints %
% ------------------------------------------ %
result.Fmul = setup.DF*Fmul(2:numnonlin+1);
% ------------------------------------------------- %
% Extract the multipliers on the linear constraints %
% ------------------------------------------------- %
result.Amul = Fmul(numnonlin+2:end);
setup = mysetup;
setup.result = result;
% ---------------------------------------------- %
% Convert the NLP back to optimal control format %
% ---------------------------------------------- %
setup = gpopsNlp2oc(setup);
setup = gpopsClearFields(setup);
output = setup;
