Use solver in C++ environment

Hi,

I use Matlab for developing and testing an acados-based trajectory planner with “external” costs (some custom casadi expression) and nonlinear constraints. Now, as it is working quite well, I want to use the solver in a C++ environment.

As a starting point I tried to run the file examples/acados_matlab_octave/test/test_template_pendulum_exact_hess.m, but I get the following error:

Invalid MEX-file '/home/martin/Programs/acados/examples/acados_matlab_octave/test/c_generated_code/acados_mex_create_pendulum_12.mexa64':
libacados_solver_pendulum_12.so: Kann die Shared-Object-Datei nicht öffnen: Datei oder Verzeichnis nicht gefunden

Error in pendulum_12_mex_solver (line 49)
            [obj.C_ocp, obj.C_ocp_ext_fun] = acados_mex_create_pendulum_12();

Error in test_template_pendulum_exact_hess (line 230)
eval( command );

The library “libacados_solver_pendulum_12.so” exists and “env.sh” has been sourced.

The script also creates an executable “main_pendulum_12”. Executing it on the Linux terminal shows a message that the solver failed:

solved ocp 20 times, solution printed above

acados_solve() failed with status 2.
iter	res_stat	res_eq		res_ineq	res_comp	qp_stat	qp_iter
0	2.983170e+03	3.222873e-34	8.000000e+01	1.926688e-13	0	0	
1	2.918394e+03	1.613408e+00	2.842171e-14	4.010854e-12	0	26	
2	4.290017e+06	7.006226e-01	2.131628e-14	6.714643e-11	0	96	
3	2.534256e+07	3.543291e-01	2.842171e-14	2.672135e-09	0	66	
4	1.099874e+06	1.625941e-01	2.842171e-14	1.533269e-10	0	19	
5	8.600711e+04	4.495132e-01	2.842171e-14	2.760462e-12	0	23	
6	7.024937e+03	4.735697e-03	2.842171e-14	8.234110e-13	0	4	
7	5.073018e+03	2.347654e-03	2.842171e-14	7.624340e-13	0	4	
8	7.789544e+03	2.473886e-03	2.842171e-14	1.766978e-09	0	3	
9	1.393847e+04	2.694400e-04	2.842171e-14	7.348139e-13	0	4	
10	8.644086e+03	1.423744e-04	2.842171e-14	7.433332e-09	0	4	
11	3.547731e+03	6.918803e-04	2.842171e-14	2.477976e-12	0	7	
12	3.478863e+03	1.326819e-03	1.421085e-14	3.615094e-13	0	5	
13	2.432628e+03	5.960200e-04	2.842171e-14	2.091413e-13	0	7	
14	2.079797e+03	1.570108e-03	2.842171e-14	3.377245e-09	0	3	
15	1.624506e+03	1.535857e-03	2.842171e-14	4.023157e-10	0	3	
16	1.916180e+03	4.048779e-03	2.842171e-14	1.231210e-10	0	3	
17	1.779184e+03	5.714423e-03	2.842171e-14	2.928617e-09	0	4	
18	1.394559e+03	1.649639e-03	2.842171e-14	1.526257e-10	0	5	
19	3.874411e+03	7.229318e-04	2.842171e-14	2.206416e-13	0	4	
20	1.434431e+03	1.509977e-02	2.842171e-14	9.879914e-11	0	6	
21	6.456054e+03	1.611307e-01	2.842171e-14	2.678858e-13	0	15	
22	2.878643e+03	8.516379e-03	2.842171e-14	2.310234e-10	0	6	
23	1.478734e+03	1.716601e-03	2.842171e-14	3.544088e-10	0	5	
24	3.431734e+03	3.470492e-03	2.842171e-14	1.747310e-13	0	4	
25	3.808968e+03	2.627217e-03	2.842171e-14	1.763849e-13	0	4	
26	1.609417e+03	2.092503e-02	2.842171e-14	1.782162e-13	0	4	
27	1.750903e+03	7.301649e-04	2.842171e-14	1.918445e-13	0	4	
28	1.751243e+03	1.117980e-03	2.842171e-14	1.282848e-10	0	3	
29	1.808320e+03	1.051385e-03	2.842171e-14	2.188382e-13	0	4	
30	0.000000e+00	0.000000e+00	0.000000e+00	0.000000e+00	0	4	

Solver info:
 SQP iterations 30
 minimum time for 20 solve 245.271000 [ms]
 KKT 1.808320e+03

Any ideas what the problem is?

BR
Martin

Hi,

does ocp.generate_c_code only work for least squares cost functions? Whenever I add some expressions to expr_ext_cost of the “pendulum_on_cart_model” and set cost_type to ext_cost or auto the code generation fails.

P.S.: I don’t need support for the other 2 issues in my last post any more

BR
Martin

Hi Martin

yes, it is not supported for external cost at the moment.
I could make some efforts to implement that.

How did you fix the previous issues?

Best,
Jonathan

Hi Jonathan,

for me it is essential to get the solver running in C++ as soon as possible. I understand if you do not have the time to solve this immediately. So could you please give me some hints in which files I have to add what?

At the moment I really don’t know where to start and what to do.

Regarding the problems in my initial post:

1st issue: Invalid MEX-File error

In Linux (Ubuntu 18.04) the current Matlab (R2019b) working directory seems to be not considered as runtime search path for libraries. However, it uses the environment variable LD_RUN_PATH. Therefore, adding export LD_RUN_PATH="$(pwd)"/c_generated_code to “env.sh” solves this problem on my machine.

2nd issue: solver in “main_pendulum_12” fails

In file “main_pendulum_12.c” one can see that the initial guess of the x-trajectory is set to zero. If it is set to the same value as in the Matlab script (“test_template_pendulum_exact_hess.m”) the solver succeeds.

BR
Martin

Hi Martin,

I quickly added support for external cost on this branch:

Please test running the following, after cleaning the build folder in the Matlab example:


using
test_template_pendulum_ocp('ext_cost')

Let me know, if it also works for your problem.

Cheers,
Jonathan

Hi Jonathan,

thank you for the quick extensions. Your example test_template_pendulum_ocp('ext_cost') works now. Unfortunately this is not the case for my own project. After calling ocp.generate_c_code I get the following output:

Warning: model parameters value cannot be defined (yet) for ocp json.
Using zeros(np,1) by default.
You can update them later using the solver object. 
> In set_up_acados_ocp_nlp_json (line 161)
  In acados_ocp/generate_c_code (line 168)
  In main (line 182) 
Reference to non-existent field 'dyn_expr_f'.

Error in set_up_acados_ocp_nlp_json (line 385)
        ocp_json.model.f_impl_expr = model.dyn_expr_f;

Error in acados_ocp/generate_c_code (line 168)
            obj.acados_ocp_nlp_json = set_up_acados_ocp_nlp_json(obj);

Error in main (line 182)
    ocp.generate_c_code

The warning at the top seems to be related to the fact that I use parameters. Is that a problem?

The error comes from using a discrete-time model. This is what I do before creating the OCP-object:

ocp_model.set('dyn_type', 'discrete');
ocp_model.set('dyn_expr_phi', model.expr_phi);

So the field “dyn_expr_f” does not exist in my case.

BR
Martin

Hi Martin,

unfortunately, discrete time formulations are not yet supported by the template based workflow.
Adding support for this will take a bit more time then for the external cost.

If you want to help, you can add a test in Matlab that first runs the native Matlab code with discrete dynamics and then tries to do the same with the template based workflow.
One will have to add some checks on the dyn_type, such that dyn_expr_f is not needed, modify the Python templates in https://github.com/acados/acados/tree/master/interfaces/acados_template/acados_template/c_templates_tera
and do some more modifications.

I will not have time for this very soon.
But if you can prepare a branch doing this as far as you get, it might speed up the process.

Best,
Jonathan

I’ve started to work on the test script in Matlab in a new local git branch. How can I share this with you?
I have no permissions to push this branch on the acados repo.

BR
Martin

Just fork acados on Github, push the branch on your fork and send me a link.

Thanks for contributing.
Merged https://github.com/acados/acados/pull/622 into master.
Now there is support of discrete dynamics from Matlab & Python :rocket:

1 Like

Hi Jonathan,

I wanted to switch back from my fork to the master branch of the official acados repo. Executing the test script “test_template_disc_dyn_ocp_linear.m” generates the following error:

Error using mex
/home/martin/Programs/acados/examples/acados_matlab_octave/test/c_generated_code/acados_mex_create_lin_mass.c: In function ‘mexFunction’:
/home/martin/Programs/acados/examples/acados_matlab_octave/test/c_generated_code/acados_mex_create_lin_mass.c:211:28: error: ‘phi_fun’
undeclared (first use in this function)
     l_ptr[0] = (long long) phi_fun;
                            ^~~~~~~
/home/martin/Programs/acados/examples/acados_matlab_octave/test/c_generated_code/acados_mex_create_lin_mass.c:211:28: note: each undeclared
identifier is reported only once for each function it appears in
/home/martin/Programs/acados/examples/acados_matlab_octave/test/c_generated_code/acados_mex_create_lin_mass.c:213:28: error: ‘phi_fun_jac_ut_xt’
undeclared (first use in this function); did you mean ‘disc_phi_fun_jac_mat’?
     l_ptr[0] = (long long) phi_fun_jac_ut_xt;
                            ^~~~~~~~~~~~~~~~~
                            disc_phi_fun_jac_mat
/home/martin/Programs/acados/examples/acados_matlab_octave/test/c_generated_code/acados_mex_create_lin_mass.c:216:28: error:
‘phi_fun_jac_ut_xt_hess’ undeclared (first use in this function); did you mean ‘phi_fun_jac_ut_xt’?
     l_ptr[0] = (long long) phi_fun_jac_ut_xt_hess;
                            ^~~~~~~~~~~~~~~~~~~~~~
                            phi_fun_jac_ut_xt


Error in make_mex_lin_mass (line 101)
            mex(FLAGS, acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...

Error in lin_mass_mex_solver (line 48)
            make_mex_lin_mass();

Error in test_template_disc_dyn_ocp_linear (line 310)
t_ocp = lin_mass_mex_solver;

Do you have an idea?

BR
Martin

Hi Martink,

thanks for reporting, I missed one file when changing the naming of some functions.

Also the tests did not fail properly on travis, which is why I missed it. I also fixed that now:

It is now properly tested by the CI and should also work on your machine.

Cheers,
Jonathan

Hi Jonathan,

for my trajectory planning algorithm I want to compare the output of the “ordinary” acados solver

ocp = acados_ocp(ocp_model, ocp_opts);

with that from the MEX-interfaced templated solver

ocp.generate_c_code;
command = strcat('t_ocp = ', ocp.model_struct.name, '_mex_solver');
eval(command);

in Matlab in order to see whether the templated version behaves roughly the same. Fortunately, the interface for setting parameters and constraints at runtime and retrieving results is identical for ocp and t_ocp and so I use exactly the same code for calling both solvers in my simulation loop. Therefore, both of them have the same constraints, weights, parameters, initial values, etc. …

I expected some minor deviations between them, since the underlying code is not exactly the same. However, I get quite different results from them. The following screenshot shows the inputs of a bicycle model from the ocp object (this is the desired behavior):

The next plot comes from t_ocp (the templated solver) and one can see that it does not even exploit the admissible input range (top plot):

So there must be a difference between ocp and t_ocp which I am not aware of. Of course analyzing the problem just with two screenshots is difficult. Do you have an assumption what I’ve forgotten here or what the reason could be?

BR
Martin

Hi Martin,

Indeed both solvers should behave the same, but are using different interfaces that were developed separately initially.
I made a few tests, that check both solvers have the same behavior up to some precision, for some test problems.

Unfortunately, I cannot say much just based on the plots.
You can explicitly initialize everything and double check, but it really seems like something else is different.

If you can share the example code with me, I am willing to look into it.

Best,
Jonathan

Hi Jonathan,

I’ve noticed in the generated C-code from the template (acados_solver_MYSOLVERNAME.c) that not all QP-solver options are set. In my fork I’ve made a branch template_mod where I changed the Matlab template workflow such that the QP solver tolerances are set correctly.

This does not reduce the deviation between the two solver results as described above, but using the same options maybe helps to isolate the problem.

The parameters qp_solver_warm_start, qp_solver_cond_ric_alg and nlp_solver_warm_start_first_qp are also missing I think. But those require changes in the template too.

What is the meaning of the parameters initialize_t_slacks and cost_numerical_hessian which are set to 0 by default?

BR
Martin

I just added qp_solver_warm_start and nlp_solver_warm_start_first_qp to the template.

Awesome!
Please open a pull request, I will try to add the remaining options and maybe polish something before merging.

Hi Martin,

cost_numerical_hessian: is an option to use a numerical hessian inside a cost module (instead of evaluating a CasADi/external function).
initialize_t_slacks: these are internal slack variables corresponding to evaluation of all inequalities (at the solution). If this option is set they are initialized by evaluating the constraints.

I think that none of these options should make a big difference in the behavior of the solver.
I would try to print the qp_in, qp_out of one problem instance into a file and have a look at the diff of these.
The input and output of the QP solver should be printed if opts->print_level > sqp_iter + 1.

Just to be sure: In both cases the solver always exits with status SUCCESS, right?

Best,
Jonathan

Ok, I will try to get that “debug output” working.

Above plots are the result from calling the solvers 280 times, from those calls in both cases (for ocp and t_ocp) about 270 exit with SUCCESS and 10 with MAX_ITER.

Hi Jonathan,

I wrote the debug outputs of ocp and t_ocp into two files for the first time-step of the simulation. Both files contain a huge amount of data and I cannot interpret most of it since I do not know enough about your solver internals.

So I used a diff-tool do find the first deviation. It is the last value of rq which is “-1” for ocp and “0” for t_ocp. Can you conclude anything from that?

For me this rq output seems to be generated before the 1st iteration of the solver. I suppose the solver starts after the lines

# it	stat		eq		ineq		comp
0	2.000000e+02	4.000000e-01	9.999964e+05	0.000000e+00.

Up to this point everything except that single rq-value is identical. After that already the 2nd line of BAbt shows small differences and from that point on pretty much everything is slightly different.

BR
Martin