Skip to content

Commit

Permalink
Merge pull request #28 from VictorManuelGracia/develop
Browse files Browse the repository at this point in the history 
Online update of MPC ingredients for laxMPC and equMPC solvers
  • Loading branch information
@pablokrupa
pablokrupa committed May 25, 2023
2 parents 2da087a + 9a7ee04 commit d8e73f9
Show file tree
Hide file tree
Showing 20 changed files with 839 additions and 758 deletions.
299 changes: 129 additions & 170 deletions types/+equMPC/code_equMPC_ADMM_C.c
View file

Large diffs are not rendered by default.

298 changes: 134 additions & 164 deletions types/+equMPC/code_equMPC_FISTA_C.c
View file

Large diffs are not rendered by default.

4 changes: 2 additions & 2 deletions types/+equMPC/compute_equMPC_ADMM_ingredients.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,8 +111,8 @@
vars.Q = -diag(Q);
vars.R = -diag(R);
end
vars.UB = UB;
vars.LB = LB;
% vars.UB = UB;
% vars.LB = LB;

% rho
if (vars.rho_is_scalar)
Expand Down
4 changes: 2 additions & 2 deletions types/+equMPC/compute_equMPC_FISTA_ingredients.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,8 +91,8 @@
vars.R = -diag(R);
vars.QRi = -[1./diag(Q); 1./diag(R)];
end
vars.UB = UB;
vars.LB = LB;
% vars.UB = UB;
% vars.LB = LB;

% Scaling vectors and operating point
if isa(controller, 'EqualityMPC')
Expand Down
23 changes: 12 additions & 11 deletions types/+equMPC/cons_equMPC_ADMM_C.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,17 +96,17 @@

% Constants
constCell = [];
if size(vars.LB, 2) > 1
% Different constraints for each prediction step
constCell = add_line(constCell, 'LB0', vars.LB(n+1:end, 1), 1, precision, var_options);
constCell = add_line(constCell, 'UB0', vars.UB(n+1:end, 1), 1, precision, var_options);
constCell = add_line(constCell, 'LB', vars.LB(:, 2:end)', 1, precision, var_options);
constCell = add_line(constCell, 'UB', vars.UB(:, 2:end)', 1, precision, var_options);
defCell = add_line(defCell, 'VAR_BOUNDS', 1, 1, 'int', 'define');
else
constCell = add_line(constCell, 'LB', vars.LB, 1, precision, var_options);
constCell = add_line(constCell, 'UB', vars.UB, 1, precision, var_options);
end
% if size(vars.LB, 2) > 1
% % Different constraints for each prediction step
% constCell = add_line(constCell, 'LB0', vars.LB(n+1:end, 1), 1, precision, var_options);
% constCell = add_line(constCell, 'UB0', vars.UB(n+1:end, 1), 1, precision, var_options);
% constCell = add_line(constCell, 'LB', vars.LB(:, 2:end)', 1, precision, var_options);
% constCell = add_line(constCell, 'UB', vars.UB(:, 2:end)', 1, precision, var_options);
% defCell = add_line(defCell, 'VAR_BOUNDS', 1, 1, 'int', 'define');
% else
% constCell = add_line(constCell, 'LB', vars.LB, 1, precision, var_options);
% constCell = add_line(constCell, 'UB', vars.UB, 1, precision, var_options);
% end
if ~solver_options.time_varying
constCell = add_line(constCell, 'Hi', vars.Hi, 1, precision, var_options);
constCell = add_line(constCell, 'Hi_0', vars.Hi_0, 1, precision, var_options);
Expand All @@ -116,6 +116,7 @@
constCell = add_line(constCell, 'Alpha', vars.Alpha, 1, precision, var_options);
constCell = add_line(constCell, 'Beta', vars.Beta, 1, precision, var_options);
end
constCell = add_line(constCell, 'dummy', 5, 1, precision, var_options);
if solver_options.in_engineering
constCell = add_line(constCell, 'scaling_x', vars.scaling_x, 1, precision, var_options);
constCell = add_line(constCell, 'scaling_u', vars.scaling_u, 1, precision, var_options);
Expand Down
23 changes: 12 additions & 11 deletions types/+equMPC/cons_equMPC_FISTA_C.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,17 +94,17 @@

% Constants
constCell = [];
if size(vars.LB, 2) > 1
% Different constraints for each prediction step
constCell = add_line(constCell, 'LB0', vars.LB(n+1:end, 1), 1, precision, var_options);
constCell = add_line(constCell, 'UB0', vars.UB(n+1:end, 1), 1, precision, var_options);
constCell = add_line(constCell, 'LB', vars.LB(:, 2:end)', 1, precision, var_options);
constCell = add_line(constCell, 'UB', vars.UB(:, 2:end)', 1, precision, var_options);
defCell = add_line(defCell, 'VAR_BOUNDS', 1, 1, 'int', 'define');
else
constCell = add_line(constCell, 'LB', vars.LB, 1, precision, var_options);
constCell = add_line(constCell, 'UB', vars.UB, 1, precision, var_options);
end
% if size(vars.LB, 2) > 1
% % Different constraints for each prediction step
% constCell = add_line(constCell, 'LB0', vars.LB(n+1:end, 1), 1, precision, var_options);
% constCell = add_line(constCell, 'UB0', vars.UB(n+1:end, 1), 1, precision, var_options);
% constCell = add_line(constCell, 'LB', vars.LB(:, 2:end)', 1, precision, var_options);
% constCell = add_line(constCell, 'UB', vars.UB(:, 2:end)', 1, precision, var_options);
% defCell = add_line(defCell, 'VAR_BOUNDS', 1, 1, 'int', 'define');
% else
% constCell = add_line(constCell, 'LB', vars.LB, 1, precision, var_options);
% constCell = add_line(constCell, 'UB', vars.UB, 1, precision, var_options);
% end
if ~solver_options.time_varying
constCell = add_line(constCell, 'AB', vars.AB, 1, precision, var_options);
constCell = add_line(constCell, 'Alpha', vars.Alpha, 1, precision, var_options);
Expand All @@ -113,6 +113,7 @@
constCell = add_line(constCell, 'R', vars.R, 1, precision, var_options);
constCell = add_line(constCell, 'QRi', vars.QRi, 1, precision, var_options);
end
constCell = add_line(constCell, 'dummy', 5, 1, precision, var_options);
if solver_options.in_engineering
constCell = add_line(constCell, 'scaling_x', vars.scaling_x, 1, precision, var_options);
constCell = add_line(constCell, 'scaling_u', vars.scaling_u, 1, precision, var_options);
Expand Down
8 changes: 4 additions & 4 deletions types/+equMPC/header_equMPC_ADMM_C.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,18 +16,18 @@ typedef struct {
#ifdef CONF_MATLAB

#if time_varying == 0
void equMPC_ADMM(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *u_opt, double *pointer_k, double *e_flag, double *z_opt, double *v_opt, double *lambda_opt, double *update_time, double *solve_time, double *polish_time, double *run_time);
void equMPC_ADMM(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *pointer_LB, double *pointer_UB, double *u_opt, double *pointer_k, double *e_flag, double *z_opt, double *v_opt, double *lambda_opt, double *update_time, double *solve_time, double *polish_time, double *run_time);
#else
void equMPC_ADMM(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *pointer_A, double *pointer_B, double *pointer_Q, double *pointer_R, double *u_opt, double *pointer_k, double *e_flag, double *z_opt, double *v_opt, double *lambda_opt, double *update_time, double *solve_time, double *polish_time, double *run_time);
void equMPC_ADMM(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *pointer_A, double *pointer_B, double *pointer_Q, double *pointer_R, double *pointer_LB, double *pointer_UB, double *u_opt, double *pointer_k, double *e_flag, double *z_opt, double *v_opt, double *lambda_opt, double *update_time, double *solve_time, double *polish_time, double *run_time);
#endif


#else

#if time_varying == 0
void equMPC_ADMM(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *u_opt, int *pointer_k, int *e_flag, solution *sol);
void equMPC_ADMM(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *pointer_LB, double *pointer_UB, double *u_opt, int *pointer_k, int *e_flag, solution *sol);
#else
void equMPC_ADMM(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *pointer_A, double *pointer_B, double *pointer_Q, double *pointer_R, double *u_opt, int *pointer_k, int *e_flag, solution *sol);
void equMPC_ADMM(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *pointer_A, double *pointer_B, double *pointer_Q, double *pointer_R, double *pointer_LB, double *pointer_UB, double *u_opt, int *pointer_k, int *e_flag, solution *sol);
#endif

#endif
Expand Down
8 changes: 4 additions & 4 deletions types/+equMPC/header_equMPC_FISTA_C.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,17 +15,17 @@ typedef struct {
#ifdef CONF_MATLAB

#if time_varying == 0
void equMPC_FISTA(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *u_opt, double *pointer_k, double *e_flag, double *z_opt, double *lambda_opt, double *update_time, double *solve_time, double *polish_time, double *run_time);
void equMPC_FISTA(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *pointer_LB, double *pointer_UB, double *u_opt, double *pointer_k, double *e_flag, double *z_opt, double *lambda_opt, double *update_time, double *solve_time, double *polish_time, double *run_time);
#else
void equMPC_FISTA(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *pointer_A, double *pointer_B, double *pointer_Q, double *pointer_R, double *u_opt, double *pointer_k, double *e_flag, double *z_opt, double *lambda_opt, double *update_time, double *solve_time, double *polish_time, double *run_time);
void equMPC_FISTA(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *pointer_A, double *pointer_B, double *pointer_Q, double *pointer_R, double *pointer_LB, double *pointer_UB, double *u_opt, double *pointer_k, double *e_flag, double *z_opt, double *lambda_opt, double *update_time, double *solve_time, double *polish_time, double *run_time);
#endif

#else

#if time_varying == 0
void equMPC_FISTA(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *u_opt, int *pointer_k, int *e_flag, sol_equMPC_FISTA *sol);
void equMPC_FISTA(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *pointer_LB, double *pointer_UB, double *u_opt, int *pointer_k, int *e_flag, sol_equMPC_FISTA *sol);
#else
void equMPC_FISTA(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *pointer_A, double *pointer_B, double *pointer_Q, double *pointer_R, double *u_opt, int *pointer_k, int *e_flag, sol_equMPC_FISTA *sol);
void equMPC_FISTA(double *pointer_x0, double *pointer_xr, double *pointer_ur, double *pointer_A, double *pointer_B, double *pointer_Q, double *pointer_R, double *pointer_LB, double *pointer_UB, double *u_opt, int *pointer_k, int *e_flag, sol_equMPC_FISTA *sol);
#endif

#endif
Expand Down
69 changes: 64 additions & 5 deletions types/+equMPC/struct_equMPC_ADMM_C_Matlab.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@ void mexFunction(int nlhs, mxArray *plhs[],
double *Q;
double *R;
#endif
double *LB; // Local LB
double *UB; // Local UB
double *u_opt; // Local u_opt
double *k; // Local k
double *e_flag; // Local e_flag
Expand All @@ -34,12 +36,12 @@ void mexFunction(int nlhs, mxArray *plhs[],

// Check number of inputs
#if time_varying == 1
if(nrhs != 7){
if(nrhs != 9){
mexErrMsgIdAndTxt("Spcies:equMPC:nrhs:number",
"Not enough inputs");
}
#else
if(nrhs != 3){
if(nrhs != 5){
mexErrMsgIdAndTxt("Spcies:equMPC:nrhs:number",
"Not enough inputs");
}
Expand Down Expand Up @@ -68,25 +70,56 @@ void mexFunction(int nlhs, mxArray *plhs[],
}

#if time_varying == 1
// Check that A is of the correct dimension
if( !mxIsDouble(prhs[3]) || mxGetNumberOfElements(prhs[3]) != nn*nn ){
mexErrMsgIdAndTxt("Spcies:laxMPC:nrhs:A",
"A must be of dimension nn by nn");
}

// Check that B is of the correct dimension
if( !mxIsDouble(prhs[4]) || mxGetNumberOfElements(prhs[4]) != nn*mm_ ){
mexErrMsgIdAndTxt("Spcies:laxMPC:nrhs:B",
"A must be of dimension nn by mm");
}


// Check that Q is of the correct dimension
if( !mxIsDouble(prhs[5]) || mxGetNumberOfElements(prhs[5]) != nn ){
mexErrMsgIdAndTxt("Spcies:laxMPC:nrhs:Q",
"Q must be a diagonal vector of nn elements");
}

// Check that R is of the correct dimension
if( !mxIsDouble(prhs[6]) || mxGetNumberOfElements(prhs[6]) != mm_ ){
mexErrMsgIdAndTxt("Spcies:laxMPC:nrhs:R",
"R must be a diagonal vector of mm elements");
}

// Check that LB is of the correct dimension
if( !mxIsDouble(prhs[7]) || mxGetNumberOfElements(prhs[7]) != nm ){
mexErrMsgIdAndTxt("Spcies:laxMPC:nrhs:LB",
"LB must be a diagonal vector of nm elements");
}

// Check that UB is of the correct dimension
if( !mxIsDouble(prhs[8]) || mxGetNumberOfElements(prhs[8]) != nm ){
mexErrMsgIdAndTxt("Spcies:laxMPC:nrhs:UB",
"UB must be a diagonal vector of nm elements");
}

#else

// Check that LB is of the correct dimension
if( !mxIsDouble(prhs[3]) || mxGetNumberOfElements(prhs[3]) != nm ){
mexErrMsgIdAndTxt("Spcies:laxMPC:nrhs:LB",
"LB must be a diagonal vector of nm elements");
}

// Check that UB is of the correct dimension
if( !mxIsDouble(prhs[4]) || mxGetNumberOfElements(prhs[4]) != nm ){
mexErrMsgIdAndTxt("Spcies:laxMPC:nrhs:UB",
"UB must be a diagonal vector of nm elements");
}

#endif

// Read input data
Expand Down Expand Up @@ -134,6 +167,32 @@ void mexFunction(int nlhs, mxArray *plhs[],
R = mxGetPr(prhs[6]);
#endif

#if MX_HAS_INTERLEAVED_COMPLEX
LB = mxGetDoubles(prhs[7]);
#else
LB = mxGetPr(prhs[7]);
#endif

#if MX_HAS_INTERLEAVED_COMPLEX
UB = mxGetDoubles(prhs[8]);
#else
UB = mxGetPr(prhs[8]);
#endif

#else

#if MX_HAS_INTERLEAVED_COMPLEX
LB = mxGetDoubles(prhs[3]);
#else
LB = mxGetPr(prhs[3]);
#endif

#if MX_HAS_INTERLEAVED_COMPLEX
UB = mxGetDoubles(prhs[4]);
#else
UB = mxGetPr(prhs[4]);
#endif

#endif

// Prepare output data
Expand Down Expand Up @@ -223,9 +282,9 @@ void mexFunction(int nlhs, mxArray *plhs[],

// Call solver
#if time_varying == 1
equMPC_ADMM(x0, xr, ur, A, B, Q, R, u_opt, k, e_flag, z_opt, v_opt, lambda_opt, update_time_sol, solve_time_sol, polish_time_sol, run_time_sol);
equMPC_ADMM(x0, xr, ur, A, B, Q, R, LB, UB, u_opt, k, e_flag, z_opt, v_opt, lambda_opt, update_time_sol, solve_time_sol, polish_time_sol, run_time_sol);
#else
equMPC_ADMM(x0, xr, ur, u_opt, k, e_flag, z_opt, v_opt, lambda_opt, update_time_sol, solve_time_sol, polish_time_sol, run_time_sol);
equMPC_ADMM(x0, xr, ur, LB, UB, u_opt, k, e_flag, z_opt, v_opt, lambda_opt, update_time_sol, solve_time_sol, polish_time_sol, run_time_sol);
#endif
}

Expand Down
46 changes: 42 additions & 4 deletions types/+equMPC/struct_equMPC_FISTA_C_Matlab.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@ void mexFunction(int nlhs, mxArray *plhs[],
double *Q;
double *R;
#endif
double *LB;
double *UB;
double *u_opt; // Local u_opt
double *k; // Local k
double *e_flag; // Local e_flag
Expand All @@ -33,12 +35,12 @@ void mexFunction(int nlhs, mxArray *plhs[],

// Check number of inputs
#if time_varying == 1
if(nrhs != 7){
if(nrhs != 9){
mexErrMsgIdAndTxt("Spcies:equMPC:nrhs:number",
"Not enough inputs");
}
#else
if(nrhs != 3){
if(nrhs != 5){
mexErrMsgIdAndTxt("Spcies:equMPC:nrhs:number",
"Not enough inputs");
}
Expand Down Expand Up @@ -86,6 +88,16 @@ void mexFunction(int nlhs, mxArray *plhs[],
mexErrMsgIdAndTxt("Spcies:laxMPC:nrhs:R",
"R must be a diagonal vector of mm elements");
}

if( !mxIsDouble(prhs[7]) || mxGetNumberOfElements(prhs[7]) != nm ){
mexErrMsgIdAndTxt("Spcies:laxMPC:nrhs:LB",
"LB must be a diagonal vector of nm elements");
}

if( !mxIsDouble(prhs[8]) || mxGetNumberOfElements(prhs[8]) != nm ){
mexErrMsgIdAndTxt("Spcies:laxMPC:nrhs:UB",
"UB must be a diagonal vector of nm elements");
}
#endif

// Read input data
Expand Down Expand Up @@ -133,6 +145,32 @@ void mexFunction(int nlhs, mxArray *plhs[],
R = mxGetPr(prhs[6]);
#endif

#if MX_HAS_INTERLEAVED_COMPLEX
LB = mxGetDouble(phrs[7]);
#else
LB = mxGetPr(prhs[7]);
#endif

#if MX_HAS_INTERLEAVED_COMPLEX
UB = mxGetDouble(phrs[8]);
#else
UB = mxGetPr(prhs[8]);
#endif

#else

#if MX_HAS_INTERLEAVED_COMPLEX
LB = mxGetDouble(phrs[3]);
#else
LB = mxGetPr(prhs[3]);
#endif

#if MX_HAS_INTERLEAVED_COMPLEX
UB = mxGetDouble(phrs[4]);
#else
UB = mxGetPr(prhs[4]);
#endif

#endif

// Prepare output data
Expand Down Expand Up @@ -214,9 +252,9 @@ void mexFunction(int nlhs, mxArray *plhs[],

// Call solver
#if time_varying == 1
equMPC_FISTA(x0, xr, ur, A, B, Q, R, u_opt, k, e_flag, z_opt, lambda_opt, update_time_sol, solve_time_sol, polish_time_sol, run_time_sol);
equMPC_FISTA(x0, xr, ur, A, B, Q, R, LB, UB, u_opt, k, e_flag, z_opt, lambda_opt, update_time_sol, solve_time_sol, polish_time_sol, run_time_sol);
#else
equMPC_FISTA(x0, xr, ur, u_opt, k, e_flag, z_opt, lambda_opt, update_time_sol, solve_time_sol, polish_time_sol, run_time_sol);
equMPC_FISTA(x0, xr, ur, LB, UB, u_opt, k, e_flag, z_opt, lambda_opt, update_time_sol, solve_time_sol, polish_time_sol, run_time_sol);
#endif
}

Expand Down

0 comments on commit d8e73f9

Please sign in to comment.