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

Use CasADi's auto-generated workspace size functions #97

Closed alphaville closed 5 years ago

alphaville commented 5 years ago

Describe the bug If MX symbols are used, it is possible that workspaces of non-zero length are needed by casadi. For example, in a test I got:

  CASADI_SYMBOL_EXPORT int phi_WodGvuyHWxVjgEOtZWoN_work(casadi_int *sz_arg, casadi_int *sz_res, casadi_int *sz_iw, casadi_int *sz_w)
  {
    if (sz_arg)
      *sz_arg = 4;
    if (sz_res)
      *sz_res = 2;
    if (sz_iw)
      *sz_iw = 0;
    if (sz_w)
      *sz_w = 12;
    return 0;
  }

Expected behavior

We should have an icasadi::init() method in Rust that will initialise these workspace lengths. ~We need a singleton to store this information.~

alphaville commented 5 years ago

Solution: keep allocation responsibility in C:

#include <stdlib.h>

#ifndef casadi_real
#define casadi_real double
#endif

#ifndef casadi_int
#define casadi_int long long int
#endif

#define TRUE 1
#define FALSE 0

extern int phi_JjQKKsHwhykwQwMSTKKI_work(
    casadi_int *sz_arg, 
    casadi_int *sz_res, 
    casadi_int *sz_iw, 
    casadi_int *sz_w);

static char is_allocated = FALSE;
static casadi_int *allocated_i_workspace;
static casadi_real *allocated_r_workspace;

static void allocate_if_not_yet() {
    casadi_int sz_arg = 0;
    casadi_int sz_res = 0;
    casadi_int sz_iw = 0;
    casadi_int sz_w = 0;
    phi_JjQKKsHwhykwQwMSTKKI_work(&sz_arg, &sz_res, &sz_iw, &sz_w);
    if (!is_allocated) {                
        allocated_i_workspace = (casadi_int*)malloc(sz_iw*sizeof(casadi_int));
        allocated_r_workspace = (casadi_real*)malloc(sz_w*sizeof(casadi_real));
    }
}

casadi_int * allocated_JjQKKsHwhykwQwMSTKKI_iwork() {   
    if (!is_allocated) allocate_if_not_yet();
    return allocated_i_workspace;
}

casadi_real * allocated_JjQKKsHwhykwQwMSTKKI_work() {   
    if (!is_allocated) allocate_if_not_yet();
    return allocated_r_workspace;
}

(todo: check the output of malloc as it may fail; btw, we should add an init method in Rust to do the memory allocation as this step may take some time)

and in Rust we simply need:

fn allocated_JjQKKsHwhykwQwMSTKKI_iwork() -> *mut c_longlong;

and function cost is implemented as follows:

pub fn cost(u: &[f64], static_params: &[f64], cost_value: &mut f64) -> i32 {
    assert_eq!(u.len(), NUM_DECISION_VARIABLES);
    assert_eq!(static_params.len(), NUM_STATIC_PARAMETERS);

    let arguments = &[u.as_ptr(), static_params.as_ptr()];
    let cost = &mut [cost_value as *mut c_double];

    unsafe {
        phi_JjQKKsHwhykwQwMSTKKI(
            arguments.as_ptr(),
            cost.as_mut_ptr(),
            allocated_JjQKKsHwhykwQwMSTKKI_iwork(),
            0 as *mut c_double,
            0 as *mut c_void,
        ) as i32
    }
}

This will keep the Rust code simpler.

alphaville commented 5 years ago

Tentative solution - next step: create Jinja template