Y_ref not available in module ocp_nlp_cost_external in c interface

User Questions
1

Hi :wave:

If using the acadaos C interface. I’m trying to replicate functionality achieved in python interface. But when I go an set the y_ref for an external trajectory I get an error:

error: model entry: y_ref not available in module ocp_nlp_cost_external

the process then dies.

The specific code is something like:

  Eigen::Matrix<double, NX, NX> Q;
        for (size_t i=0; i< NX; i++)
        {
            Q.diagonal()[i] = Q_trace[i];
        }
        Eigen::Matrix<double, NU, NU> R;
        for (size_t i =0; i< NU; i++)
        {
            R.diagonal()[i] = 0.01;
        }

        Eigen::Matrix<double, NX+NU, NX+NU> W;
        W.block(0,0,NX, NX) = Q;
        W.block(NX, NX, NU, NU)=R;

        RCLCPP_INFO(rclcpp::get_logger("mpc_node"), "Setting Cost" );

        for (size_t i =0; i < N_S; i++)
        {
            RCLCPP_INFO(rclcpp::get_logger("mpc_node"), "Setting Costi %d", i );
            ocp_nlp_cost_model_set(nlp_config_, nlp_dims_, nlp_in_, i,
                    "y_ref", (double*)yref[i].data() );
             //  --> Code dies here
            ocp_nlp_cost_model_set(nlp_config_, nlp_dims_, nlp_in_, i,
                    "W", (double*)W.data() );
        }

        RCLCPP_INFO(rclcpp::get_logger("mpc_node"), "Setting End Cost" );
        ocp_nlp_cost_model_set(nlp_config_, nlp_dims_, nlp_in_, N_S,
                    "y_ref_e", (double*)yref[N_S].data() );
        ocp_nlp_cost_model_set(nlp_config_, nlp_dims_, nlp_in_, N_S,
                    "W", (double*)Q.data() );

and is similar to how a reference is done in the engine example in the acados c example directory. However I initialise my nlp config using the generated functions such as

acados_ocp_capsule = <name>_acados_create_capsule();
<name>_acados_create(acados_ocp_capsule);
nlp_config_ = <name>_get_nlp_config(acados_ocp_capsule);

This is different to that used in the engine example which is full of boilerplate.
I am assuming most of this is unnecessary if development is done in python and autogenerated of a model <name>. I am assuming that the auto generated code pre configures the nlp via the

<name>_acados_get_<this_component>(acados_ocp_capsule)

where <this_component> is some part of the nlp problem?

How do I ensure that the capsule is initialised correctly to accept an external y_ref?

2

It seems that the generated code removes the requirement for most of the boilerplate which is great.

I seems that I was able to get yref to be recognised if in the .json, definition “ny” > 0
and the default values for “yref” are established, which I did by defining them in the python script. A gotcha for me was that I initially had configured an External Cost (which I think was equivalent to a non-linear cost) and as a result “ny” was 0 in the External Cost definition, but set to the size of yref when using NONLINEAR_LS.

This is probably desirable, but how would you set it in the case of an “EXTERNAL” cost defintion, directly?

Anyway, it got through setting “yref” and now is complaining about “yref_e” not being available for nonlinear_ls e.g.

error: field yref_e not available in ocp_nlp_cost_nls_model_set

is it not valid to set end constraints for nonlinear_ls?

3

I think that “yref_e” doesn’t exist, just use “yref” and set the last index I think is the way forward.

The Model doesn’t converge even though it is using similar parameters to that in python, will have to unit test it, currently getting an infeasible solution:

QP solver returned error status 36 in SQP iteration 1, QP iteration 50.
multicopter_angle_control_acados_solve() failed with status 4.
iter	res_stat	res_eq		res_ineq	res_comp	qp_stat	qp_iter	alpha
0	1.441917e+247	9.806600e-01	2.000000e+01	0.000000e+00	0	0	0.000000e+00	
1	0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	36	50	0.000000e+00	

error: ocp_nlp_get_at_stage: field u not available
4

Finally working. The infeasible solution was caused by a seemingly too small R cost matrix value (the cost weight for inputs u). had them about 200 times smaller than the values in the Qmatrix. By adjusting these to 100 times smaller made the solver succeed.

I’ve checked and other larger values are fine too.

5

In fact it wasn’t finally working, just not crashing :grinning:. The u outputs were not valid and weren’t abiding the umax and umin limits.

The key thing that I had overlooked was that setting yref vector was ny in length which was nx + nu. I had only been setting the trajectory part nx, leaving the input part uninitialized. So once I had increased yref to include the u components as zero. The optimisation output correct values, and the solver optimised to success.