Issue with interface.c

[alphaville]

Describe the bug

I noticed that when F1 and F2 depend on p, although the algorithm converges, there are issues with termination. After digging a little deeper, I realised that parameter p is actually not passed to F1 and F2 properly. I was able to locate the issue in interface.c - see below for details.

To Reproduce

Minimum working example:

import casadi.casadi as cs
import opengen as og

# -- Let's see what's wrong...

u = cs.SX.sym("u", 5, 1)
p = cs.SX.sym("u", 3, 1)

f = og.functions.rosenbrock(u, cs.vertcat(p[0], p[1]))
U = og.constraints.BallInf(None, 1)

F2 = u[0] - p[2]

problem = og.builder.Problem(u, p, f).with_constraints(U).with_penalty_constraints(F2)

build_config = og.config.BuildConfiguration()\
                .with_build_directory("python_build")\
                .with_build_mode("debug")\
                .with_tcp_interface_config()

meta = og.config.OptimizerMeta()\
                .with_optimizer_name("funky_tester")

solver_config = og.config.SolverConfiguration()\
                .with_tolerance(1e-6)\
                .with_initial_tolerance(1e-6)

og.builder.OpEnOptimizerBuilder(problem, meta, build_config, solver_config).build()

mng = og.tcp.OptimizerTcpManager("python_build/funky_tester")
mng.start()
solver_status = mng.call([1, 1, 0.5])
print(solver_status)
mng.kill()

Expected behavior

In the above MWE, we obtain a solution with u[0] being very close to 0.5, however, the value of F2 at the solution is 0.49999. It turns out that the reason is how we copy p in interface.c (lines 261 and 276).

Additional context

I'll fix it in a new branch.

[alphaville]

Corrected versions of F1 and F2 in interface.c:

/* ------MAPPING F1------------------------------------------------------------- */

int mapping_f1_function(const casadi_real** arg, casadi_real** res) {
    const casadi_real* args__[F1_SZ_ARG] = {uxip_space, uxip_space + NU + NXI};
    copy_args_into_up_space(arg);
    result_space_f1[0] = res[0];
    return {{build_config.alm_mapping_f1_function_name}}(
        args__,
        result_space_f1,
        allocated_i_workspace_f1,
        allocated_r_workspace_f1,
        (void*) 0);
}

/* ------MAPPING F2------------------------------------------------------------- */

int mapping_f2_function(const casadi_real** arg, casadi_real** res) {
    const casadi_real* args__[F2_SZ_ARG] = {uxip_space, uxip_space + NU + NXI};
    copy_args_into_up_space(arg);
    result_space_f2[0] = res[0];
    return {{build_config.constraint_penalty_function_name}}(
        args__,
        result_space_f2,
        allocated_i_workspace_f2,
        allocated_r_workspace_f2,
        (void*) 0);
}