How could I pass the MX variable as a double variable?

[zahra boroujeni]

Hi all, 

I want to pass the distance between current position and desired position as a term of the cost function for an nonlinear problem. I can just calculate the distance of function using an internal library. The input of the function should be double variables, while the X[k] is a MX variable. Do you have any solution or idea?

Please find below the relevant part of the code.

Best,

```

class MyCallback : public Callback {

 private:

   // Data members

   mpc_obj mpc_obj1;

   // Private constructor

   MyCallback(mpc_obj mpc_obj) : mpc_obj1(mpc_obj) {}

 public:

   // Creator function, creates an owning reference

   static Function create(const std::string& name, mpc_obj mpc_obj,

                          const Dict& opts=Dict()) {

     return Callback::create(name, new MyCallback(mpc_obj), opts);

   }

   // Initialize the object

   virtual void init() {

     std::cout << "initializing object" << std::endl;

   }

   // Number of inputs and outputs

   virtual int get_n_in() { return 2;}

   virtual int get_n_out() { return 2;}

   // Evaluate numerically

    virtual std::vector<MX> eval(std::vector<MX>& arg) {

    MX x = arg.at(0);

    MX y = arg.at(1);

    Position pos_car;

pos_car[0] = (double) x * boost::units::si::meter;

pos_car[1] = (double) y * boost::units::si::meter;

    Length par=mpc_obj1.drive->findClosestParameter(pos_car);

    pos_car=mpc_obj1.leftBoundary->interpolate(par);

    MX f = {pos_car[0].value(),pos_car[1].value()};

    return {f};

   }

 };

main

{

     Function f = MyCallback::create("f", mpc_obj);

    vector<MX> arg={X[k][0],X[k][1]};

    std::vector<MX> res = f(arg);

    //std::cout << res << std::endl;

MX driveSpline=0.01*(pow(X[k][0]-res.at(0),2)+pow(X[k][1]-res.at(0),2));

J += I_out.at("qf")+driveSpline;

}

// NLP 

MXDict nlp = {{"x", V}, {"f", J}, {"g", vertcat(g)}};

// Set options

Dict opts;

opts["ipopt.tol"] = 1e-5;

opts["ipopt.max_iter"] = 200;

//opts["ipopt.linear_solver"] = "ma27";

// Create an NLP solver and buffers

Function solver = nlpsol("nlpsol", "ipopt", nlp, opts);

std::map<std::string, DM> arg, res;

}

```

Error

```

terminate called after throwing an instance of 'casadi::CasadiException'

  what():  The assertion "hasDerivative()" on line 2427 of file "/home/mi/boroujeni/casadi/casadi/core/function/function_internal.cpp" failed. 

Please notify the CasADi developers.

```

[Joel Andersson]

Hi,

Your code above doesn't make much sense. The "MX" type is a symbolic expression. It cannot be converted to a "double" variable in general. Did you read through the user guide (guide.casadi.org) or look at the examples? For the setpoints, you might be able to treat them as parameters ("p" keyword) for the NLP solver object.

Best regards,

Joel

[zahra boroujeni]

Hi Joel,

What is you mentioned is exactly my problem. I need to call a function through iterations. For example we can pass the "MX" type to "sin()" function, I need to pass the "MX" type to "myfunction", how could I do that? As the input of "sin()" , the input of "myfunction()" should be double. 

Yes I read the user guide, and I think parameter would not help me, because the parameter should be some how independent! Do you have any idea? 

Best,

Zahra

[Joris Gillis]

Dear Zahra,

Just glancing on your code; don't you intend to implement the std::vector<DM> variant of eval?
Especially since the comment says "Evaluate numerically"..

Best,
  Joris

[zahra boroujeni]

Hi Joris,

Sorry, I think, by adding my code I just confused every body, I copied this part of code from the cplusplus examples and the comments are not relevant. 

Best,

Zahra

[Joris Gillis]

Okay but have you tried changing the signature to std::vector<double> ?
I don't think the std::vector<MX> case is relevant for you..

[zahra boroujeni]

I tried, but I received the same error.

You mean like that:

virtual std::vector<double> eval(std::vector<double>& arg) {

   double x = arg.at(0);

   double y = arg.at(1);

   Position pos_car;

pos_car[0] = (double) x * boost::units::si::meter;

pos_car[1] = (double) y * boost::units::si::meter;

   Length par=mpc_obj1.drive->findClosestParameter(pos_car);

   pos_car=mpc_obj1.leftBoundary->interpolate(par);

   std::vector<double>  f = {pos_car[0].value(),pos_car[1].value()};

   return f;

[zahra boroujeni]

Also with DM , I received the same error after compiling.

Please see the whole code below:

https://github.com/ZahraBoroujeni/mpc_planner/blob/master/src/main/main.cpp

[Joris Gillis]

You're close:

virtual std::vector<DM> eval(std::vector<DM>& arg) {

   double x = arg.at(0).scalar();

   double y = arg.at(1).scalar();

}


or

virtual std::vector<DM> eval(std::vector<DM>& arg) {

   double x = static_cast<double>(arg.at(0));

   double y = static_cast<double>(arg.at(1));

}

The reason why you get the error is because `operator double() const;` of DM is defined 'explicit'..

Best,
  Joris

[zahra boroujeni]

But I received the same error! It crashed again.

[zahra boroujeni]

It is the terminal output, it carshes while try to solve the nlp:

___________________________________________________

initializing object

MX(f(V[0], V[1]){1})

MX(f(V[6], V[7]){1})

MX(f(V[12], V[13]){1})

MX(f(V[18], V[19]){1})

MX(f(V[24], V[25]){1})

MX(f(V[30], V[31]){1})

MX(f(V[36], V[37]){1})

MX(f(V[42], V[43]){1})

MX(f(V[48], V[49]){1})

MX(f(V[54], V[55]){1})

MX(f(V[60], V[61]){1})

MX(f(V[66], V[67]){1})

MX(f(V[72], V[73]){1})

MX(f(V[78], V[79]){1})

MX(f(V[84], V[85]){1})

MX(f(V[90], V[91]){1})

MX(f(V[96], V[97]){1})

MX(f(V[102], V[103]){1})

MX(f(V[108], V[109]){1})

MX(f(V[114], V[115]){1})

MX(f(V[120], V[121]){1})

MX(f(V[126], V[127]){1})

MX(f(V[132], V[133]){1})

MX(f(V[138], V[139]){1})

MX(f(V[144], V[145]){1})

MX(f(V[150], V[151]){1})

MX(f(V[156], V[157]){1})

MX(f(V[162], V[163]){1})

MX(f(V[168], V[169]){1})

MX(f(V[174], V[175]){1})

MX(f(V[180], V[181]){1})

MX(f(V[186], V[187]){1})

MX(f(V[192], V[193]){1})

MX(f(V[198], V[199]){1})

MX(f(V[204], V[205]){1})

MX(f(V[210], V[211]){1})

MX(f(V[216], V[217]){1})

MX(f(V[222], V[223]){1})

MX(f(V[228], V[229]){1})

MX(f(V[234], V[235]){1})

MX(f(V[240], V[241]){1})

MX(f(V[246], V[247]){1})

MX(f(V[252], V[253]){1})

MX(f(V[258], V[259]){1})

MX(f(V[264], V[265]){1})

MX(f(V[270], V[271]){1})

MX(f(V[276], V[277]){1})

MX(f(V[282], V[283]){1})

MX(f(V[288], V[289]){1})

MX(f(V[294], V[295]){1})

terminate called after throwing an instance of 'casadi::CasadiException'

  what():  The assertion "hasDerivative()" on line 2427 of file "/home/mi/boroujeni/casadi/casadi/core/function/function_internal.cpp" failed. 

Please notify the CasADi developers.

[fub_mpc_planner-1] process has died [pid 12054, exit code -6, cmd /home/mi/boroujeni/fub_roscar/devel/lib/fub_mpc_planner/main __name:=fub_mpc_planner __log:=/home/mi/boroujeni/.ros/log/61783088-9144-11e6-a46b-64006a78d270/fub_mpc_planner-1.log].

log file: /home/mi/boroujeni/.ros/log/61783088-9144-11e6-a46b-64006a78d270/fub_mpc_planner-1*.log

[zahra boroujeni]

sorry, but I did not understand your point! 

Where should I add the jacobian, in the MyCallback? 

   virtual bool has_jacobian() const;
   virtual Function get_jacobian(const std::string& name, const Dict& opts);

But why should I add jacobian for the function?!  The jacobian should be zero!

[Joris Gillis]

Dear Zahra,

Your knowledge that 'jacobian should be zero!' must be encoded in CasADi.

Simple add these two in MyCallback indeed.

has_jacobian should return true
get_jacobian should return a Function that is R x R -> zeros(1,2):
SX x = SX::sym('x');
SX y = SX::sym('y');
return Function('f',{x,y},{DM::zeros(1,2)});
(Untested code)

Btw, feel welcome to make pull requests to improve our examples/documentation..

Best,
   Joris

[zahra boroujeni]

Dear Joris,

I defined also Jacobean function as below, but still I received the same error!

 virtual bool has_jacobian() {return true;}

   virtual Function get_jacobian(const std::string& name, const Dict& opts){

SX x  = SX::sym("x");

SX y  = SX::sym("y");

return Function("f",{x,y},{DM::zeros(1,2)});

}

 

Best,

Zahra

[Joris Gillis]

Try rc3.1.0rc1 or even develop branch.

[zahra boroujeni]

Hi Joris, 

I tried with the master and release-3.1.0-rc1, but I recieved the same error! I compiled the casadi using gcc 4.8

terminate called after throwing an instance of 'casadi::CasadiException'

  what():  The assertion "hasDerivative()" on line 2575 of file "../casadi-release-3.1.0-rc1/casadi/core/function/function_internal.cpp" failed. 

Please notify the CasADi developers.

[zahra boroujeni]

It seems that I should use FunctionInternal which has "eval_mx"!!

[Joris Gillis]

Dear Zahra,

This should be your final implementation (compiles and runs for me):


   // Evaluate numerically
   virtual std::vector<DM> eval(const std::vector<DM>& arg) {

    double x = arg.at(0).scalar();
    double y = arg.at(1).scalar();

    ....
    return
   }
  
   virtual bool has_jacobian() const { return true; }

   virtual Function get_jacobian(const std::string& name, const Dict& opts) {
    SX x = SX::sym("x");
    SX y = SX::sym("y");

    return Function("f",{x,y},{DM::zeros(2, 2)});
   }


Note the use of 'const' in the argument of eval, and the 'const' in the has_jacobian line.
It's easy to miss these (missed them myself at first try).

Best regards,
  Joris

[zahra boroujeni]

Hi Joris,

Yes, that works now.

Thank you,

Best,

Zahra

[zahra boroujeni]

adding  "override"  after defining the function would help to find the bug faster! 

[Andres Codas Duarte]

Hi,

   I found this quite useful.  I did a python implementation for a similar functionality:

class _value(Callback):
    def __init__(self, sp, opts={}):
        Callback.__init__(self)
        self.sp = sp
        self.Js = []
        self.construct('Constant_Value', opts)

    def get_n_in(self):
        return 1

    def get_n_out(self):
        return 1

    def get_sparsity_in(self, *args):
        return Sparsity.dense(self.sp)

    def get_sparsity_out(self, *args):
        return Sparsity.dense(self.sp)

    def has_jacobian(self):
        return True

    def get_n_forward(self):
        return 0

    def get_n_reverse(self, *args):
        return 0

    def get_jacobian(self, name, options):
        X = SX.sym('X', self.sp)
        V = X*0
        J = Function('name', [X], [jacobian(V, X)], options)
        self.Js.append(J)
        return J

    def eval(self, args):
        return [args[0]]

   Is there any function in the API that I'm not aware of with this functionality.  The issue is that it is quite annoying to the need to keep in scope the instance of the "_value" and its Jacobian.  If I don't keep any reference to them the object is destroyed.

    Regards,
    Andres