Manual Jacobians instead of Algorithmic differentiation

mindthomas · July 20, 2020, 8:23am

Hi.
Is it possible to manually provide the Jacobians for the dynamics, objective function and constraint functions.
In my particular the Jacobians have been derived and implemented in C manually for efficiency and I am looking for a way to provide these to Acados.

Best regards
Thomas Jespersen

FreyJo · July 21, 2020, 9:14am

Hi,

It is definitely possible in general from a C part of acados.
The following file is relevant for this:
Mostly external_function_generic is realized by external_function_casadi.

github.com

acados/acados/blob/53e9a5933b755c42aeea6c1912825a9ee93c6a88/acados/utils/external_function_generic.h#L107


    void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **);
	// public members for interfaces
    void (*get_nparam)(void *, int *);
    void (*set_param)(void *, double *);
    // private members
    void *ptr_ext_mem;  // pointer to external memory
    int (*fun)(void **, void **, void *);
    double *p;  // parameters
    int np;     // number of parameters
    // .....
} external_function_param_generic;

//
int external_function_param_generic_struct_size();
//
void external_function_param_generic_set_fun(external_function_param_generic *fun, void *value);
//
int external_function_param_generic_calculate_size(external_function_param_generic *fun, int np);
//
void external_function_param_generic_assign(external_function_param_generic *fun, void *mem);
//

If you can call the C function with CasADi variables, you could try to adapt this example:

That being said, this is obviously a more involved way of using acados.
It also depends on which jacobians you want to provide manually and which interface you want to use.

I hope this helps anyway.

Best,
Jonathan

mindthomas · July 21, 2020, 3:09pm

Hi Jonathan.
Thanks for the quick reply. The external_function_param_generic interface is definitely in the right direction.

Ideally, what I’m looking is just a way to supply the cost function and its Jacobian as pure C functions.
That is, be able to define the pure C code behind model.cost_y_expr and model.cost_y_expr_e with no library dependencies, no algorithmic differentiation etc.

With Forces this was not quite possible, but I was able to achieve something very close by using their External Function interface: 9. High-level Interface — FORCESPRO 6.3.0 documentation

void myfunctions (
    double *x, /* primal vars */
    double *y, /* eq. constraint multiplers */
    double *l, /* ineq . constraint multipliers */
    double *p, /* runtime parameters */
    double *f, /* objective function ( incremented in this function ) */
    double *nabla_f , /* gradient of objective function */
    double *c, /* dynamics */
    double *nabla_c , /* Jacobian of the dynamics ( column major ) */
    double *h, /* inequality constraints */
    double *nabla_h , /* Jacobian of inequality constraints ( column major ) */
    double *H, /* Hessian ( column major ) */
    int stage, /* stage number (0 indexed ) */
    int iteration /* Solver iteration count */
)

Although this requires everything to be specified in that C function it is acceptable for now.

So the question is if something similar can be achieved with Acados? So without using CasADi.
Both the example you provided and what I have found at https://github.com/acados/acados/tree/master/experimental/interfaces/matlab/external_function_generation is using CasADi. For the latter, is the generated code used in another example anywhere?

Maybe I can achieve the above with ocp_nlp_cost_external_model_set with the field set to ext_cost_fun and ext_cost_fun_jac respectively? If so I would greatly appreciate some directions or hints, especially on the prerequisites before calling this function?
Or maybe this is exactly the example I am looking for: https://github.com/acados/acados/blob/master/examples/c/no_interface_examples/mass_spring_nmpc_example.c#L158-L262 ?

PS: The main reason for not using CasADi is that I’m unable to use CasADi since the function evaluations relies on an external array of data - which I don’t expect to be able to set as an online parameter

Again, thanks for your help.

Kind regards
Thomas Jespersen

FreyJo · July 31, 2020, 11:10am

Hi Thomas,

sorry for the late reply.
I was not aware of this interface in Forces, but it would be really nice to have something like this in acados.

As for now, this is unfortunately not the case, and one would has to provide functions that follow the conventions of the corresponding acados module, similarly to what you found here: https://github.com/acados/acados/blob/master/examples/c/no_interface_examples/mass_spring_nmpc_example.c#L158-L262
This will not be a bit different for the dynamics.
Regarding them: Do you want to use an acados integrator or provide discrete dynamics.

Note, there was some effort to document the convention on external functions of the modules: https://github.com/acados/acados/blob/interface_fixes/utils/convention_about_external_functions.txt

As for prerequisites on how to set the model function, you indeed have to just supply an object of type external_function_generic that implements the evaluate function.

github.com

acados/acados/blob/edf9c1fcccd3f9c275ee7fd6d0020f9933b61c38/examples/c/no_interface_examples/mass_spring_nmpc_example.c#L502-L503


      
          external_function_generic ext_cost_generic;
          ext_cost_generic.evaluate = &ext_cost;

FreyJo · July 31, 2020, 11:11am

Why do you think you cannot supply this data as online parameters?

mindthomas · August 30, 2020, 7:26am

Why does ext_cost_fun have to be generated and assigned when ext_cost_fun_jac_hess is used?
https://github.com/acados/acados/blob/master/interfaces/acados_matlab_octave/ocp_set_ext_fun.m#L401
I have been able to successfully leave these out and still be able to compile and run the solver.

What is the minimum requirements in terms of cost function? Can it work with just ext_cost_fun or does it need at least the Jacobian or is the Hessian also a requirement?
What performance differences should one expect when using ext_cost vs auto? I have tried to switch between the two for the minimal_example_ocp.m and it does lead to quite different trajectories:

With ocp_model.set('cost_type', 'auto') and ocp_model.set('cost_type_e', 'auto'):

With ocp_model.set('cost_type', 'ext_cost') and ocp_model.set('cost_type_e', 'ext_cost'):

Isn’t this line supposed to be multiplication by 0.5 instead of addition? https://github.com/acados/acados/blob/master/examples/c/no_interface_examples/mass_spring_nmpc_example.c#L203

Finally I am currently prototyping a MATLAB interface to add support for custom cost functions defined in C. The current work is far from done, but so far I have managed to get it to work with Casadi exported cost functions but when I try to use the generic cost interface and the definition from https://github.com/acados/acados/blob/master/examples/c/no_interface_examples/mass_spring_nmpc_example.c#L158-L262 the solver crashes.
I would appreciate if you could take a look and possibly try it locally: Initial prototype of MATLAB interface for external cost in C · mindThomas/acados@69375d9 · GitHub
The test is implemented as part of the minimal_example_ocp.m example where you can set the custom external cost type with: generic_or_casadi

Thanks in advance.

Regards Thomas

FreyJo · August 31, 2020, 8:50am

It might be that ext_cost_fun is needed here, even though this is not called by default.
Does that answer your question?

github.com

acados/acados/blob/37b1851433cb62aead85e1597b68e3a08cc973ae/acados/ocp_nlp/ocp_nlp_cost_external.c#L716


      
          // OUTPUT
          ext_fun_type_out[0] = COLMAJ;
          ext_fun_out[0] = &memory->fun;  // function: scalar
          
          // evaluate external function
          if (model->ext_cost_fun == 0)
          {
              printf("ocp_nlp_cost_external_compute_fun: ext_cost_fun is not provided. Exiting.\n");
              exit(1);
          }
          model->ext_cost_fun->evaluate(model->ext_cost_fun, ext_fun_type_in, ext_fun_in,
                                        ext_fun_type_out, ext_fun_out);
          
          // slack update function value
          blasfeo_dveccpsc(2*ns, 2.0, &model->z, 0, &work->tmp_2ns, 0);
          blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->tmp_ux, nu+nx, &work->tmp_2ns, 0);
          memory->fun += 0.5 * blasfeo_ddot(2*ns, &work->tmp_2ns, 0, memory->tmp_ux, nu+nx);
          
          // scale
          if(model->scaling!=1.0)
          {

It needs the Jacobian, but it can work without the Hessian.
There is an option to use a user defined approximation of the hessian contribution of the cost function and only evaluate the function and its Jacobian.
However, this is only interfaced to Python for now.

github.com

acados/acados/blob/37b1851433cb62aead85e1597b68e3a08cc973ae/acados/ocp_nlp/ocp_nlp_cost_external.c#L622


      
          ext_fun_in[0] = &x_in;
          ext_fun_type_in[1] = BLASFEO_DVEC_ARGS;
          ext_fun_in[1] = &u_in;
          
          // OUTPUT
          ext_fun_type_out[0] = COLMAJ;
          ext_fun_out[0] = &memory->fun;  // fun: scalar
          ext_fun_type_out[1] = BLASFEO_DVEC;
          ext_fun_out[1] = &memory->grad;  // grad: nu+nx
          
          if (opts->use_numerical_hessian > 0)
          {
              // evaluate external function
              model->ext_cost_fun_jac->evaluate(model->ext_cost_fun_jac, ext_fun_type_in,
                                                  ext_fun_in, ext_fun_type_out, ext_fun_out);
              // custom hessian
              blasfeo_dgead(nx+nu, nx+nu, model->scaling, &model->numerical_hessian, 0, 0, memory->RSQrq, 0, 0);
          }
          else
          {
              // additional output

For me both cost types yield a trajectory that looks like this:
Did you try this on master with a fresh build?
ref_traj

In general setting cost_type to auto, attempts to equivalently reformulate the general nonlinear cost into a linear least squares formulation and uses the external cost module otherwise.

Right, I guess it should be a multiplication.

I will try to comment on your other question later today.

Best,
Jonathan

FreyJo · September 2, 2020, 10:19am

Hi Thomas,

I had a look on what you did on branch mindThomas/manual_external_cost_matlab.

The error I get is:

Stack Trace (from fault):
[  0] 0x00007efcaaf4c254 /home/oj/acados/examples/acados_matlab_octave/getting_started/build/manual_pendulum_ocp_set_ext_fun_cost_0_fun_jac_hess.mexa64+00004692
[  1] 0x00007efcaafe1556                   /home/oj/acados/lib/libacados.so+00443734 external_function_param_generic_wrapper+00000073
[  2] 0x00007efcaafa7dba                   /home/oj/acados/lib/libacados.so+00208314 ocp_nlp_cost_external_update_qp_matrices+00000550
[  3] 0x00007efcaaf9c9cc                   /home/oj/acados/lib/libacados.so+00162252 ocp_nlp_approximate_qp_matrices+00000980
[  4] 0x00007efcaafb920e                   /home/oj/acados/lib/libacados.so+00279054 ocp_nlp_sqp+00004641
[  5] 0x00007efcaafed698                   /home/oj/acados/lib/libacados.so+00493208 ocp_nlp_solve+00000093

I think that external_function_param_generic_wrapper should call the C function ext_cost instead of manual_pendulum_ocp_set_ext_fun_cost_0_fun_jac_hess. Does that make sense?

Moreover, I see that you just overwrite the pointers in C_ocp_ext_fun.
I think it would be better to directly use your manual cost function at creation time, within the constructor ocp = acados_ocp(ocp_model, ocp_opts);
Overwriting the pointers, makes it impossible to free the corresponding memory later on.

Sorry that I cannot give a full solution for this.
Developing/debugging this is quite time consuming.

I hope this is anyway helpful for you…

Cheers,
Jonathan

mindthomas · September 2, 2020, 2:26pm

Thanks for looking into it.
It should be correct that it is trying to call the manual_pendulum_ocp_set_ext_fun_cost_0_fun_jac_hess.mexa64 file since this is the one that contains the compiled version of ext_cost. If you do an objdump on that mex file you will see that it both includes the mex function from ocp_set_ext_fun_generic.c together with ext_cost and ext_costN. However, I am not sure that the pointer is correct.
On the other hand the approach is working with the casadi-generated external cost code

I think it would be better to directly use your manual cost function at creation time, within the constructor ocp = acados_ocp(ocp_model, ocp_opts);

I totally agree and this is also my intention. As I mentioned the work is far from done and I am still exploring how to set this external cost. When I have identified and confirmed the correct approach I will be cleaning up the code and integrating it properly.

Regards Thomas

mindthomas · September 2, 2020, 2:53pm

Problem solved. Apparently the mex function in ocp_set_ext_fun_generic.c calls the external_function_param_generic_set_fun function which sets a function, fun with a signature:
void ext_cost(void **in, void **out, void *params)
which is different from the signature of evaluate
void ext_cost(void *fun, ext_fun_arg_t *type_in, void **in, ext_fun_arg_t *type_out, void **out)
which is the one being set in the no interface, external cost example.

With the correct signature it works. I have pushed the changes but am also working on a better solution to avoid the memory leak.

Regards Thomas

mindthomas · September 2, 2020, 4:01pm

I have now integrated the manual external cost source file selection properly with the solver generation interface.
Feel free to have a look and let me know what you think: https://github.com/mindThomas/acados/blob/5df7d1c1a8e3d4c7691199614b5e4ddf7651ce85/examples/acados_matlab_octave/getting_started/minimal_example_ocp.m#L77-L87

FreyJo · September 2, 2020, 4:58pm

That looks good!
It seems like you have a good picture of the Matlab interface by now
Let me know, if you are really stuck or done, so I will have another look.
Good luck till then

mindthomas · September 5, 2020, 10:27am

I have now prepared a PR with my changes: https://github.com/acados/acados/pull/634/