mex: setters for general linear constraints

acados · Oct 9, 2019 · 8fc943f · 8fc943f
1 parent 737698b
commit 8fc943f
Show file tree

Hide file tree

Showing 4 changed files with 68 additions and 11 deletions.
diff --git a/examples/acados_matlab_octave/test/test_ocp_pendulum_on_cart.m b/examples/acados_matlab_octave/test/test_ocp_pendulum_on_cart.m
@@ -258,6 +258,8 @@
         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
 

diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c
@@ -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],

diff --git a/interfaces/acados_matlab_octave/ocp_create.c b/interfaces/acados_matlab_octave/ocp_create.c
@@ -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);

diff --git a/interfaces/acados_matlab_octave/ocp_set.c b/interfaces/acados_matlab_octave/ocp_set.c
@@ -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;
@@ -117,13 +122,54 @@ 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_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"))
     {
@@ -438,7 +484,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_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");