alphaville / optimization-engine

Nonconvex embedded optimization: code generation for fast real-time optimization + ROS support
https://alphaville.github.io/optimization-engine/
Other
512 stars 53 forks source link

Preconditioning: implementation in interface.c #288

Closed Arun-R-M closed 2 years ago

Arun-R-M commented 2 years ago

Main Changes

Associated Issues

TODOs

alphaville commented 2 years ago

I believe it's a good idea to test interface.c by itself (to write a standalone test just for this file). A bad and lazy way to do so is to put a main file in interface.c and compile using

gcc -Wall interface.c \
   auto_casadi_cost.c \
   auto_casadi_mapping_f1.c \
   auto_casadi_mapping_f2.c \
   auto_preconditioning_functions.c \
   auto_casadi_grad.c \
   casadi_memory.h

A good way to do this is to write a cmake file.

alphaville commented 2 years ago

Example of calling w_cost

int main(void)
{
    casadi_real u[NU_ROSENBROCK] = {1, 2, 3, 4, 5.5};
    casadi_real p[NP_ROSENBROCK] = {1, 10};
    casadi_real r[1];
    const casadi_real *args[2] = {u, p};
    casadi_real *res[1] = {r};
    mapping_w_cost_function_rosenbrock(args, res);
    printf("w_cost = %g\n", r[0]);
    return 0;
}
alphaville commented 2 years ago

See our discussion in the chat for details. I'm posting this recommendation here for reference:

int mapping_w1_function_python_bindings(const casadi_real **arg, casadi_real **res)
{
    int status = 0;
#if N1_PYTHON_BINDINGS > 0
    /* Array of pointers to where (u, p) are stored */
    const casadi_real *args__[W1_SZ_ARG_PYTHON_BINDINGS] =
        {uxip_space,                                             /* :u   */
         uxip_space + NU_PYTHON_BINDINGS + NXI_PYTHON_BINDINGS}; /* :p   */
    /* Copy given data to variable `uxip_space` */
    copy_args_into_up_space(arg);
    /*
     * The result should be written in result_space_w1
     * (memory has been allocated - see beginning of this file)
     */
    result_space_w1[0] = res[0];
    /*
     * Call auto-generated function
     * Implemented in: icasadi/extern/auto_preconditioning_functions.c
     */
    status = open_mapping_w_f1_python_bindings(
        args__,
        result_space_w1,
        allocated_i_workspace_w1,
        allocated_r_workspace_w1,
        (void *)0);
#endif
return status;
}
alphaville commented 2 years ago

@Arun-R-M I wrote a test for interface.c which revealed an issue with the computation of w1 and w2. Let us not test init_penalty before we have fixed this.

Firstly, this is how to run the test: firstly, run main.c. This will create the optimiser my_optimizers/rosenbrock. Navigate to my_optimizers/rosenbrock/icasadi[...]/extern and run

clang -Wall -DTEST_INTERFACE interface.c  \
  auto_casadi_cost.c  \
  auto_casadi_mapping_f1.c  \
  auto_casadi_mapping_f2.c  \
  auto_preconditioning_functions.c  \
  auto_casadi_grad.c

and run the binary file that this generates (e.g., a.out on Linux and MacOS).

Here is the issue: w1 and w2 are vectors, not scalars. This is how w1 is computed now:

jac_constraint_f1 = OpEnOptimizerBuilder.__casadi_norm_infinity(cs.jacobian(c_f1, u).T)
w_constraint_f1_fn = cs.Function(meta.w_f1_function_name, [u, p], [1 / cs.fmax(1, jac_constraint_f1)])

This is wrong!

alphaville commented 2 years ago

@Arun-R-M @smokinmirror I have written some tests which you can run as I have explained above. Can you please check whether all scaling parameters are computed correctly? Then, let us all do a code review so we can merge this.