function setup = gpopsGetBounds(setup);
%------------------------------------------------------------------%
% Get all bounds in a multiple-phase optimal control problem %
%------------------------------------------------------------------%
% GPOPS Copyright (c) Anil V. Rao, Geoffrey T. Huntington, David %
% Benson, Michael Patterson, Christopher Darby, & Camila Francolin %
%------------------------------------------------------------------%
minima = setup.limits{1};
maxima = setup.limits{2};
sizes = setup.sizes;
numphases = size(sizes,1);
nodes = setup.nodes;
variable_offset = 0;
constraint_offset = 0;
state_matrix_min = [];
state_matrix_max = [];
control_matrix_min = [];
control_matrix_max = [];
parameter_min = [];
parameter_max = [];
path_matrix_min = [];
path_matrix_max = [];
event_vector_min = [];
event_vector_max = [];
for iphase=1:numphases;
% ---------------------------------------------------------
% Get the lower and upper limits on the initial and terminal
% time in the current phase
% ---------------------------------------------------------
t0_min = minima{iphase,1}(1);
t0_max = maxima{iphase,1}(1);
tf_min = minima{iphase,1}(2);
tf_max = maxima{iphase,1}(2);
% -------------------------------------------------------------------
% Get the lower and upper limits on the states in the current phase
% -------------------------------------------------------------------
if ~isequal(sizes(iphase,1),0),
state_matrix_min(1,:) = minima{iphase,2}(:,1);
state_matrix_min(2:nodes(iphase)+1,:) = repmat(minima{iphase,2}(:,2),1,nodes(iphase)).';
state_matrix_min(nodes(iphase)+2,:) = minima{iphase,2}(:,3);
state_matrix_max(1,:) = maxima{iphase,2}(:,1);
state_matrix_max(2:nodes(iphase)+1,:) = repmat(maxima{iphase,2}(:,2),1,nodes(iphase)).';
state_matrix_max(nodes(iphase)+2,:) = maxima{iphase,2}(:,3);
% -------------------------------------------------------------------
% Check the lower and upper limits on the states in the current phase
% -------------------------------------------------------------------
if ~isempty(find(maxima{iphase,2}(:,1) - minima{iphase,2}(:,2) < 0, 1)) || ...
~isempty(find(maxima{iphase,2}(:,3) - minima{iphase,2}(:,2) < 0, 1)) || ...
~isempty(find(maxima{iphase,2}(:,2) - minima{iphase,2}(:,1) < 0, 1)) || ...
~isempty(find(maxima{iphase,2}(:,2) - minima{iphase,2}(:,3) < 0, 1)) || ...
~isempty(find(maxima{iphase,2}(:,:) - minima{iphase,2}(:,:) < 0, 1))
error('State bounds inconsistant (i.e. max < min) in phase: %i',iphase)
end
else
state_matrix_min = [];
state_matrix_max = [];
end;
% -------------------------------------------------------------------
% Get the lower and upper limits on the controls in the current phase
% -------------------------------------------------------------------
if ~isequal(sizes(iphase,2),0),
control_matrix_min(1:nodes(iphase),:) = repmat(minima{iphase,3},1,nodes(iphase)).';
control_matrix_max(1:nodes(iphase),:) = repmat(maxima{iphase,3},1,nodes(iphase)).';
% -------------------------------------------------------------------
% Check the lower and upper limits on the controls in the current phase
% -------------------------------------------------------------------
if ~isempty(find(maxima{iphase,3}(:,:) - minima{iphase,3}(:,:) < 0, 1))
error('Control bounds inconsistant (i.e. max < min) in phase: %i',iphase)
end
else
control_matrix_min = [];
control_matrix_max = [];
end;
% ----------------------------------------------------------------------------
% Get the lower and upper limits on the static parameters in the current phase
% ----------------------------------------------------------------------------
if ~isequal(sizes(iphase,3),0),
parameter_min = minima{iphase,4};
parameter_max = maxima{iphase,4};
% -------------------------------------------------------------------
% Check the lower and upper limits on the static parameters in the current phase
% -------------------------------------------------------------------
if ~isempty(find(maxima{iphase,4}(:,:) - minima{iphase,4}(:,:) < 0, 1))
error('Parameter bounds inconsistant (i.e. max < min) in phase: %i',iphase)
end
else
parameter_min = [];
parameter_max = [];
end;
% ---------------------------------------------------------------------------
% Get the lower and upper limits on the path constraints in the current phase
% ---------------------------------------------------------------------------
if ~isequal(sizes(iphase,4),0),
path_matrix_min(1:nodes(iphase),:) = repmat(minima{iphase,5},1,nodes(iphase)).';
path_matrix_max(1:nodes(iphase),:) = repmat(maxima{iphase,5},1,nodes(iphase)).';
% -------------------------------------------------------------------
% Check the lower and upper limits on the path constraints in the current phase
% -------------------------------------------------------------------
if ~isempty(find(maxima{iphase,5}(:,:) - minima{iphase,5}(:,:) < 0, 1))
error('Path constraint bounds inconsistant (i.e. max < min) in phase: %i',iphase)
end
else
path_matrix_min = [];
path_matrix_max = [];
end;
% ----------------------------------------------------------------------------
% Get the lower and upper limits on the event constraints in the current phase
% ----------------------------------------------------------------------------
if ~isequal(sizes(iphase,5),0),
event_vector_min = minima{iphase,6};
event_vector_max = maxima{iphase,6};
% -------------------------------------------------------------------
% Check the lower and upper limits on the event constraints in the current phase
% -------------------------------------------------------------------
if ~isempty(find(maxima{iphase,4}(:,:) - minima{iphase,4}(:,:) < 0, 1))
error('Event constraint bounds inconsistant (i.e. max < min) in phase: %i',iphase)
end
else
event_vector_min = [];
event_vector_max = [];
end;
state_vector_min = state_matrix_min(:);
state_vector_max = state_matrix_max(:);
control_vector_min = control_matrix_min(:);
control_vector_max = control_matrix_max(:);
ode_vector_min = zeros((nodes(iphase)+1)*sizes(iphase,1),1);
ode_vector_max = zeros((nodes(iphase)+1)*sizes(iphase,1),1);
path_vector_min = path_matrix_min(:);
path_vector_max = path_matrix_max(:);
% ------------------------------------------------------------------------------------------------------
% The cell array NLPLIMITS contains the lower and upper limits on the NLP variables in the current phase
% ------------------------------------------------------------------------------------------------------
nlplimits{iphase,1} = [state_vector_min; control_vector_min; t0_min; tf_min; parameter_min];
nlplimits{iphase,2} = [state_vector_max; control_vector_max; t0_max; tf_max; parameter_max];
nlplimits{iphase,3} = [ode_vector_min; path_vector_min; event_vector_min];
nlplimits{iphase,4} = [ode_vector_max; path_vector_max; event_vector_max];
variables(iphase) = length(nlplimits{iphase,1});
constraints(iphase) = length(nlplimits{iphase,3});
variable_indices{iphase} = variable_offset+1:variable_offset+variables(iphase);
constraint_indices{iphase} = constraint_offset+1:constraint_offset+constraints(iphase);
nstates = sizes(iphase,1);
ncontrols = sizes(iphase,2);
nparameters = sizes(iphase,3);
state_indices = 1:(nodes(iphase)+2)*nstates;
control_indices = state_indices(end)+1:state_indices(end)+nodes(iphase)*ncontrols;
if ~isempty(control_indices),
t0_index = control_indices(end)+1;
else
t0_index = state_indices(end)+1;
end;
tf_index = t0_index+1;
parameter_indices = tf_index+1:tf_index+nparameters;
indices(iphase).states = state_indices;
indices(iphase).controls = control_indices;
indices(iphase).time = [t0_index; tf_index];
indices(iphase).parameters = parameter_indices;
variable_offset = variable_offset+variables(iphase);
constraint_offset = constraint_offset+constraints(iphase);
state_matrix_min = [];
state_matrix_max = [];
control_matrix_min = [];
control_matrix_max = [];
parameter_min = [];
parameter_max = [];
path_matrix_min = [];
path_matrix_max = [];
event_vector_min = [];
event_vector_max = [];
end;
connections = setup.connections;
numconnections = size(connections,1);
linkmin = [];
linkmax = [];
for iconnect=1:numconnections;
linkmin = [linkmin; connections{iconnect,1}];
linkmax = [linkmax; connections{iconnect,2}];
% -------------------------------------------------------------------
% Check the lower and upper limits on the linkage constraints
% -------------------------------------------------------------------
if ~isequal(size(connections{iconnect,1}),size(connections{iconnect,2}))
error(['Linkage Upper and Lower Bound Vector Must Be Same Size\n\t',...
'i.e. size(setup.connections{%i,1}) == size(setup.connections{%i,2})'],iconnect,iconnect);
end
if ~isempty(find(connections{iconnect,1} -connections{iconnect,2} < 0, 1))
error('Linkage constraint bounds inconsistant (i.e. max < min) in connection: %i',iconnect)
end
if isequal(connections{iconnect,3}(1),connections{iconnect,3}(2)) || ...
connections{iconnect,3}(1) <= 0 || connections{iconnect,3}(1) > numphases || ...
connections{iconnect,3}(2) <= 0 || connections{iconnect,3}(2) > numphases
error('Invalid Linkage phase numbers in connection: %i',iconnect)
end
end;
numlinks = length(linkmin);
varbounds_min = vertcat(nlplimits{:,1});
varbounds_max = vertcat(nlplimits{:,2});
conbounds_min = vertcat(nlplimits{:,3});
conbounds_max = vertcat(nlplimits{:,4});
conbounds_min = [conbounds_min; linkmin];
conbounds_max = [conbounds_max; linkmax];
setup.varbounds_min = varbounds_min;
setup.varbounds_max = varbounds_max;
setup.conbounds_min = conbounds_min;
setup.conbounds_max = conbounds_max;
setup.nlplimits = nlplimits;
setup.variables = variables;
setup.constraints = constraints;
setup.variable_indices = variable_indices;
setup.constraint_indices = constraint_indices;
setup.numphases = numphases;
setup.numnonlin = length(conbounds_min);
setup.numlinks = numlinks;
setup.indices = indices;
% --------------------------------
% Get Bounds on Linear Constraints
% --------------------------------
numvars = length(varbounds_min);
Alinear = zeros(numphases+numconnections,numvars);
Alinear = sparse(Alinear);
Alinmin = zeros(numphases+numconnections,1);
Alinmax = zeros(numphases+numconnections,1);
% ---------------------------------------------
% Part 1: Monotonicity of Independent Variable
% ---------------------------------------------
for iphase=1:numphases;
nstates = sizes(iphase,1);
ncontrols = sizes(iphase,2);
% nparameters = sizes(iphase,3);
if ~isequal(iphase,1),
ishift = variable_indices{iphase-1}(end);
else
ishift =0;
end;
t0_index = ishift+(nodes(iphase)+2)*nstates+nodes(iphase)*ncontrols+1;
tf_index = t0_index+1;
Alinear(iphase,t0_index) = -1;
Alinear(iphase,tf_index) = 1;
if size(minima,2)>6,
if ~isempty(minima{iphase,7})
Alinmin(iphase) = minima{iphase,7};
else
Alinmin(iphase) = 0;
end;
else
Alinmin(iphase) = 0;
end;
if size(maxima,2)>6,
if ~isempty(maxima{iphase,7})
Alinmax(iphase) = maxima{iphase,7};
else
Alinmax(iphase) = Inf;
end;
else
Alinmax(iphase) = Inf;
end;
end;
istart = numphases;
% --------------------------------------
% Part 2: Linkage of Time Across Phases
% --------------------------------------
for iconnect=1:numconnections;
left_phase = connections{iconnect,3}(1);
right_phase = connections{iconnect,3}(2);
nparameters_left = sizes(left_phase,3);
nparameters_right = sizes(right_phase,3);
tf_index_left = variable_indices{left_phase}(end-nparameters_left);
t0_index_right = variable_indices{right_phase}(end-nparameters_right-1);
Alinear(istart+iconnect,tf_index_left) = -1;
Alinear(istart+iconnect,t0_index_right) = 1;
Alinmin(istart+iconnect) = 0;
Alinmax(istart+iconnect) = 0;
end;
setup.numconnections = numconnections;
setup.numvars = numvars;
setup.numlin = length(Alinmin);
setup.Alinear = Alinear;
setup.Alinmin = Alinmin;
setup.Alinmax = Alinmax;
