Skip to content

Commit

Permalink
Merge pull request #489 from FreyJo/master
Browse files Browse the repository at this point in the history 
Mex setters for general linear constraints
  • Loading branch information
@FreyJo
FreyJo committed Oct 9, 2019
2 parents 7ea4b24 + 3d0ec92 commit 16058c4
Show file tree
Hide file tree
Showing 6 changed files with 126 additions and 34 deletions.
1 change: 1 addition & 0 deletions ROADMAP.md
View file
Expand Up @@ -3,6 +3,7 @@
#### core
- [ ] propagate cost in integrator
- [ ] restore download and compilation of OOQP
- [ ] split ocp solve into prepare and feedback

#### documentation
- [ ] provide OCP NLP formulation that is handled by `ocp_nlp` as a formula in docs
Expand Down
1 change: 1 addition & 0 deletions examples/acados_matlab_octave/pendulum_on_cart_model/minimal_example_ocp.m
View file
Expand Up @@ -76,6 +76,7 @@
ocp_model.set('cost_expr_ext_cost_e', model.expr_ext_cost_e);

%% constraints
nbu = nu;
Jbu = zeros(nbu, nu); for ii=1:nbu Jbu(ii,ii)=1.0; end
lbu = -80*ones(nu, 1);
ubu = 80*ones(nu, 1);
Expand Down
61 changes: 38 additions & 23 deletions examples/acados_matlab_octave/test/test_ocp_pendulum_on_cart.m
View file
Expand Up @@ -98,10 +98,6 @@

nbx = 0;
nbu = 0;
ng = 0;
ng_e = 0;
nh = nu;
nh_e = 0;

% cost
Vu = zeros(ny, nu); for ii=1:nu Vu(ii,ii)=1.0; end % input-to-output matrix in lagrange term
Expand Down Expand Up @@ -138,10 +134,7 @@
end
ocp_model.set('dim_nbx', nbx);
ocp_model.set('dim_nbu', nbu);
ocp_model.set('dim_ng', ng);
ocp_model.set('dim_ng_e', ng_e);
ocp_model.set('dim_nh', nh);
ocp_model.set('dim_nh_e', nh_e);

% symbolics
ocp_model.set('sym_x', model.sym_x);
if isfield(model, 'sym_u')
Expand Down Expand Up @@ -173,15 +166,36 @@
ocp_model.set('dyn_type', 'implicit');
ocp_model.set('dyn_expr_f', model.expr_f_impl);
end
% constraints

%% constraints
ocp_model.set('constr_x0', x0);
ocp_model.set('constr_expr_h', model.expr_h);
ocp_model.set('constr_lh', lbu);
ocp_model.set('constr_uh', ubu);
if itest == 1
nh = nu;
ng = 0;
ocp_model.set('constr_expr_h', model.expr_h);
ocp_model.set('constr_lh', lbu);
ocp_model.set('constr_uh', ubu);
else
nh = 0;
ng = 1;
C = zeros(ng, nx);
D = zeros(ng, nu);
D(1, nu) = 1;
ocp_model.set('constr_D', D);
ocp_model.set('constr_C', C);

ocp_model.set('constr_lg', lbu);
ocp_model.set('constr_ug', ubu);
end
ng_e = 0;
nh_e = 0;
ocp_model.set('dim_ng', ng);
ocp_model.set('dim_ng_e', ng_e);
ocp_model.set('dim_nh', nh);
ocp_model.set('dim_nh_e', nh_e);
% disp('ocp_model.model_struct')
% disp(ocp_model.model_struct)


%% acados ocp opts
ocp_opts = acados_ocp_opts();
ocp_opts.set('compile_interface', compile_interface);
Expand Down Expand Up @@ -215,18 +229,10 @@
ocp_opts.set('gnsf_detect_struct', gnsf_detect_struct);
end

% disp('ocp_opts');
% disp(ocp_opts.opts_struct);


%% acados ocp
% create ocp
ocp = acados_ocp(ocp_model, ocp_opts);
% disp('ocp.C_ocp');
% disp(ocp.C_ocp);
% disp('ocp.C_ocp_ext_fun');
% disp(ocp.C_ocp_ext_fun);


% set trajectory initialization
%x_traj_init = zeros(nx, N+1);
Expand All @@ -243,12 +249,18 @@
% modify numerical data for a certain stage
some_stages = 1:10:N-1;
for i = some_stages
if (strcmp(cost_type, 'linear_ls'))
if (strcmp(cost_type, 'linear_ls'))
ocp.set('cost_Vx', Vx, i); % cost_y_ref, cost_Vu, cost_Vx, cost_W, cost_Z, cost_Zl,...
% cost_Zu, cost_z, cost_zl, cost_zu;
ocp.set('cost_Vu', Vu, i);
ocp.set('cost_y_ref', yr, i);
end
if ng > 0
ocp.set('constr_C', C, i);
ocp.set('constr_D', D, i);
ocp.set('constr_ug', ubu, i);
ocp.set('constr_lg', lbu, i);
end
end

% solve
Expand Down Expand Up @@ -283,14 +295,17 @@
end
end


if status~=0
error('test_ocp_pendulum_on_cart: solution failed!');
elseif tol < max(stat(end,2:5))
error('test_ocp_pendulum_on_cart: residuals bigger than tol!');
elseif sqp_iter > 9
error('test_ocp_pendulum_on_cart: sqp_iter > 9, this problem is typically solved within less iterations!');
end
% For debugging
% figure;
% plot(1:N+1, xtraj);
% legend('p', 'theta', 'v', 'omega');

end
fprintf('\ntest_ocp_pendulum_on_cart: success!\n');
6 changes: 6 additions & 0 deletions interfaces/acados_c/ocp_nlp_interface.c
View file
Expand Up @@ -549,6 +549,12 @@ int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_n
"nbu", &dims_value);
return dims_value;
}
else if (!strcmp(field, "lg") || !strcmp(field, "ug"))
{
config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage],
"ng", &dims_value);
return dims_value;
}
else if (!strcmp(field, "s"))
{
config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage],
Expand Down
22 changes: 12 additions & 10 deletions interfaces/acados_matlab_octave/ocp_create.c
View file
Expand Up @@ -1608,26 +1608,28 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])


// C
if (mxGetField( matlab_model, 0, "constr_C" )!=NULL)
const mxArray *C_matlab = mxGetField( matlab_model, 0, "constr_C" );
if (C_matlab != NULL)
{
int matlab_size = (int) mxGetNumberOfElements( mxGetField( matlab_model, 0, "constr_C" ) );
int acados_size = ng*nx;
MEX_DIM_CHECK_VEC(fun_name, "constr_C", matlab_size, acados_size);
int nrow = (int) mxGetM( C_matlab );
int ncol = (int) mxGetN( C_matlab );
MEX_DIM_CHECK_MAT(fun_name, "constr_C", nrow, ncol, ng, nx);

double *C = mxGetPr( mxGetField( matlab_model, 0, "constr_C" ) );
double *C = mxGetPr( C_matlab );
for (int ii=0; ii<N; ii++)
{
ocp_nlp_constraints_model_set(config, dims, in, ii, "C", C);
}
}
// D
if (mxGetField( matlab_model, 0, "constr_D" )!=NULL)
const mxArray *D_matlab = mxGetField( matlab_model, 0, "constr_D" );
if (D_matlab!=NULL)
{
int matlab_size = (int) mxGetNumberOfElements( mxGetField( matlab_model, 0, "constr_D" ) );
int acados_size = ng*nu;
MEX_DIM_CHECK_VEC(fun_name, "constr_D", matlab_size, acados_size);
int nrow = (int) mxGetM( D_matlab );
int ncol = (int) mxGetN( D_matlab );
MEX_DIM_CHECK_MAT(fun_name, "constr_D", nrow, ncol, ng, nu);

double *D = mxGetPr( mxGetField( matlab_model, 0, "constr_D" ) );
double *D = mxGetPr( D_matlab );
for (int ii=0; ii<N; ii++)
{
ocp_nlp_constraints_model_set(config, dims, in, ii, "D", D);
Expand Down
69 changes: 68 additions & 1 deletion interfaces/acados_matlab_octave/ocp_set.c
View file
Expand Up @@ -83,7 +83,12 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
char *field = mxArrayToString( prhs[4] );
// value
double *value = mxGetPr( prhs[5] );

// for checks
int matlab_size = (int) mxGetNumberOfElements( prhs[5] );
int nrow = (int) mxGetM( prhs[5] );
int ncol = (int) mxGetN( prhs[5] );

// mexPrintf("\nocp_set: %s, matlab_size %d\n", field, matlab_size);

int N = dims->N;
Expand Down Expand Up @@ -117,13 +122,74 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
int NN[] = {N, 1}; // size of phases, i.e. shooting nodes with same dimensions

/* Set value */
// constraints
if (!strcmp(field, "constr_x0"))
{
acados_size = nx;
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
ocp_nlp_constraints_model_set(config, dims, in, 0, "lbx", value);
ocp_nlp_constraints_model_set(config, dims, in, 0, "ubx", value);
}
else if (!strcmp(field, "constr_C"))
{
for (int ii=s0; ii<se; ii++)
{
int ng = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
MEX_DIM_CHECK_MAT(fun_name, "constr_C", nrow, ncol, ng, nx);

ocp_nlp_constraints_model_set(config, dims, in, ii, "C", value);
}
}
else if (!strcmp(field, "constr_lbx"))
{
for (int ii=s0; ii<se; ii++)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lbx");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);

ocp_nlp_constraints_model_set(config, dims, in, ii, "lbx", value);
}
}
else if (!strcmp(field, "constr_ubx"))
{
for (int ii=s0; ii<se; ii++)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ubx");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);

ocp_nlp_constraints_model_set(config, dims, in, ii, "ubx", value);
}
}
else if (!strcmp(field, "constr_D"))
{
for (int ii=s0; ii<se; ii++)
{
int ng = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
MEX_DIM_CHECK_MAT(fun_name, "constr_D", nrow, ncol, ng, nu);

ocp_nlp_constraints_model_set(config, dims, in, ii, "D", value);
}
}
else if (!strcmp(field, "constr_lg"))
{
for (int ii=s0; ii<se; ii++)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lg");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);

ocp_nlp_constraints_model_set(config, dims, in, ii, "lg", value);
}
}
else if (!strcmp(field, "constr_ug"))
{
for (int ii=s0; ii<se; ii++)
{
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);

ocp_nlp_constraints_model_set(config, dims, in, ii, "ug", value);
}
}
// cost:
else if (!strcmp(field, "cost_y_ref"))
{
Expand Down Expand Up @@ -438,7 +504,8 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
}
else
{
MEX_FIELD_NOT_SUPPORTED_SUGGEST(fun_name, field, "p, constr_x0, cost_y_ref[_e],\
MEX_FIELD_NOT_SUPPORTED_SUGGEST(fun_name, field, "p, constr_x0,\
constr_lbx, constr_ubx, constr_C, constr_D, constr_lg, constr_ug, cost_y_ref[_e],\
cost_Vu, cost_Vx, cost_Vz, cost_W, cost_Z, cost_Zl, cost_Zu, cost_z,\
cost_zl, cost_zu, init_x, init_u, init_z, init_xdot, init_gnsf_phi,\
init_pi, nlp_solver_max_iter, qp_warm_start, warm_start_first_qp");
Expand Down

0 comments on commit 16058c4

Please sign in to comment.