Could not get the expected output

Question

Problme:

I won't get the result that I get from Matlab implementation, I am not sure pow_pos(norm(x(:, 1) - start'), 2) I have converted correctly, here is the converted code

      Variable::t x_0 = Var::vstack(x->index(0, 0),  x->index(1, 0), x->index(2, 0));
      M->constraint(Expr::vstack(t_tmp->index(0), Expr::sub(x_0, start_)), Domain::inQCone());
      M->constraint(Expr::hstack(t->index(0), 1, t_tmp->index(0)), Domain::inPPowerCone(1.0/2));

Output

Here black dot represents what I get from Matlab and green dots depicts what I get from C++

Here is the original code I wrote in Matlab which gives the expected output

    clear all
close all
clc

number_of_steps = 15;
lambda3_val = 1000;
lambda1_val = 1000;
lambda2_val = 0.1;
dim_ = 3;
Ab = [-0.470233  0.882543         0   3.21407
 0.470233 -0.882543        -0  0.785929
-0.807883 -0.430453  0.402535   4.81961
 0.807883  0.430453 -0.402535  -1.40824
-0.355254 -0.189285 -0.915405  0.878975
 0.355254  0.189285  0.915405   1.12103];

A = Ab(:,1:dim_);
b = Ab(:,dim_+1);

start = [-4 , 0, 2];
goal = [-1, -1, 1];

nPolytopes = 1;
free_space = polytope(A, b);

cvx_solver mosek
cvx_begin
    variable x(3, number_of_steps)
    binary variable c(nPolytopes, number_of_steps);
    cost = 0;
    for i = 1:(number_of_steps-1)
        cost = cost + pow_pos(norm(x(:, i) - x(:, i+1), 1),2)*lambda2_val;
    end
    cost = cost + pow_pos(norm(x(:, 1) - start'), 2)*lambda1_val;
    cost = cost + pow_pos(norm(x(:, number_of_steps) - goal'),2)*lambda3_val;
    minimize(cost)
    subject to
        for i = 1:number_of_steps
            A*x(:, i) <= b;
        end
cvx_end

Here is the code the conversion of the above into c++ using Mosek

#include <iostream>
#include "fusion.h"

using namespace mosek::fusion;
using namespace monty;

int main(int argc, char ** argv)
{

  int number_of_steps = 15;
  int lambda1_val = 1000;
  int lambda2_val = 1000;
  int lambda3_val = 0.1;
  int dim_space = 3;
  auto A = new_array_ptr<double, 2>({{-0.4702,    0.8825,         0},
                                      {0.4702,   -0.8825,         0},
                                      {-0.8079,   -0.4305,    0.4025},
                                      {0.8079,    0.4305,   -0.4025},
                                      {-0.3553,   -0.1893,   -0.9154},
                                      {0.3553,    0.1893,    0.9154}});
  
  auto b = new_array_ptr<double, 1>({3.2141,
                                    0.7859,
                                    4.8196,
                                  -1.4082,
                                    0.8790,
                                    1.1210});

  auto end_ = new_array_ptr<double, 1>({-1, -1, -1});
  auto start_ = new_array_ptr<double, 1>({-4, 0, 2});

  Model::t M = new Model();
  auto x = M->variable(new_array_ptr<int,1>({dim_space, number_of_steps}), Domain::unbounded()) ;
  auto t = M->variable(number_of_steps, Domain::unbounded());
  auto t_tmp = M->variable(number_of_steps, Domain::unbounded());
  auto lambda_1 = M->parameter("lambda_1");
  auto lambda_2 = M->parameter("lambda_2");
  auto lambda_3 = M->parameter("lambda_3");
  
  Variable::t x_0 = Var::vstack(x->index(0, 0),  x->index(1, 0), x->index(2, 0));
  M->constraint(Expr::vstack(t_tmp->index(0), Expr::sub(x_0, start_)), Domain::inQCone());
  M->constraint(Expr::hstack(t->index(0), 1, t_tmp->index(0)), Domain::inPPowerCone(1.0/2));

  for(int i=1; i<number_of_steps-1; i++){
    Variable::t x_i = Var::vstack(x->index(0,i),  x->index(1,i), x->index(2,i));
    Variable::t x_i1 = Var::vstack(x->index(0,i+1),  x->index(1,i+1), x->index(2,i+1));
    M->constraint(Expr::vstack(t_tmp->index(i), Expr::sub(x_i1, x_i)), Domain::inQCone());
    M->constraint(Expr::hstack(t->index(i), 1, t_tmp->index(i)), Domain::inPPowerCone(1.0/2));
  }

  Variable::t x_n = Var::vstack(x->index(0, number_of_steps-1),  x->index(1, number_of_steps-1), x->index(2, number_of_steps-1));
  M->constraint(Expr::vstack(t_tmp->index(number_of_steps-1), Expr::sub(x_n, end_)), Domain::inQCone());
  M->constraint(Expr::hstack(t->index(number_of_steps-1), 1, t_tmp->index(number_of_steps-1)), Domain::inPPowerCone(1.0/2));

  for (int i = 0; i < number_of_steps; i++)
  {
    auto x_i = Var::vstack(x->index(0,i), x->index(1, i), x->index(2, i));
    M->constraint(Expr::mul(A, x_i), Domain::lessThan(b));
  }

  M->setLogHandler([](const std::string& msg){std::cout<< msg << std::flush;});

  auto lambda1 = M->getParameter("lambda_1");
  auto lambda2 = M->getParameter("lambda_2");
  auto lambda3 = M->getParameter("lambda_3");
  lambda1->setValue(lambda1_val);
  lambda2->setValue(lambda2_val);
  lambda3->setValue(lambda3_val);
              
  auto objs = new_array_ptr<Expression::t, 1>(number_of_steps);
  (*objs)[0] = (Expr::mul(lambda1, t->index(0)));
  for(int i=1; i<number_of_steps-1; i++){
    (*objs)[i] = Expr::mul(lambda2, t->index(i));
  }
  (*objs)[number_of_steps-1] = Expr::mul(lambda3, t->index(number_of_steps-1));

  M->objective(ObjectiveSense::Minimize, Expr::add(objs));

  M->solve();
  auto sol = *(x->level());
  std::cout<< "solution "<< sol << std::endl;

}

And when you used your debugger to run your program step by step, one line at a time, and monitor the values of all variables, and seeing how they change after every statement, at which point did those values started to differ from the expected results, and what was the reason for that? — Sam Varshavchik, Jun 15 '21 at 12:18
@SamVarshavchik , it just not a debugging problem, there is no one to one mapping between them, the problem formulation is different, I have found out where is the problem lies, which mentioned at the end. Without understand don't just try to close the question? — GPrathap, Jun 15 '21 at 12:23
Every C++ runtime problem is a debugging problem, by definition. — Sam Varshavchik, Jun 15 '21 at 12:23
If there is no one to one mapping between your two problems, why are they both here? Your text seems to suggest that the second one is a version of the first. In any case, indeed you need some debugging here. — Ander Biguri, Jun 15 '21 at 12:29
Maybe add what output you got and what you've expected instead. — Michael Mahn, Jun 15 '21 at 12:31
I dont know what to say, but I clearly mentioned where the problems lie. I am new to Mosek, no confident what I did is correct. that what I am asking, no idea why taking about debugging (; — GPrathap, Jun 15 '21 at 12:37
The reason there's a talk about debugging is because that's the process to figure out why a C++ program is not getting the expected results. Only very obvious problems can be eyeballed and spotted simply by looking at the code, and that's a very small percentage -- only the most obvious bugs. For all other issues debugging is required. Knowing how to effectively use a debugger is a required skill for every C++ developer, no exceptions. Anyone who wants to be good at C++ needs to learn how to debug. — Sam Varshavchik, Jun 15 '21 at 12:40
I answered you on the Mosek google group. The best starting point is to write one or both models to a file and compare if they correspond to what you wanted to model. — Michal Adamaszek, Jun 15 '21 at 12:45
@MichalAdamaszek thank you, I will update once I have some results — GPrathap, Jun 15 '21 at 12:49
I resolved all your issues in https://groups.google.com/g/mosek/c/35JseJyY8cU — Michal Adamaszek, Jun 16 '21 at 08:45
@MichalAdamaszek yeap, thank you so much, could you pose your answer here, I will accept it — GPrathap, Jun 16 '21 at 09:22

score 1 · Answer 1 · answered Jun 29 '21 at 10:59

With the help of @michal-adamaszek and the answer given in the Mosek Google Group (https://mail.google.com/mail/u/0/#inbox/FMfcgzGkXmgmHqFBVJFSNRWmjQfQSPlg) Here is the working solution for the above problem,

#include <iostream>
#include "fusion.h"

using namespace mosek::fusion;
using namespace monty;

#define nint1(a)  new_array_ptr<int>({(a)})
#define nint(a,b) new_array_ptr<int>({(a),(b)})

int main(int argc, char ** argv)
{

  int number_of_steps = 15;
  double lambda1_val = 1000;
  double lambda2_val = 0.1;
  double lambda3_val = 1000;
  int dim_space = 3;
  auto A = new_array_ptr<double, 2>({{-0.470233,    0.882543,         0},
                                      {0.470233,   -0.882543,         0},
                                      {-0.807883,   -0.430453,    0.402535},
                                      {0.807883,    0.430453,   -0.402535},
                                      {-0.355254,   -0.189285,   -0.915405},
                                      {0.355254,    0.189285,    0.915405}});
 
  auto b = new_array_ptr<double, 1>({3.21407,
                                    0.785929,
                                    4.81961,
                                  -1.40824,
                                    0.878975,
                                    1.12103});

  auto end_ = new_array_ptr<double, 1>({-1, -1, 1});
  auto start_ = new_array_ptr<double, 1>({-4, 0, 2});


  Model::t M = new Model();
  auto x = M->variable("x", new_array_ptr<int,1>({dim_space, number_of_steps}), Domain::unbounded()) ;
  auto t      = M->variable("t", number_of_steps-1, Domain::unbounded());
  auto tstart = M->variable("ts",Domain::unbounded());
  auto tend   = M->variable("te",Domain::unbounded());

  M->constraint(Expr::vstack(tstart, 0.5, Expr::sub(x->slice(nint(0,0), nint(3,1))->reshape(nint1(3)),
                                                    start_)), 
                Domain::inRotatedQCone());

  M->constraint(Expr::hstack(t, 
                             Expr::constTerm(number_of_steps-1, 0.5), 
                             Expr::transpose(Expr::sub(x->slice(nint(0,0), nint(3,number_of_steps-1)),
                                                       x->slice(nint(0,1), nint(3,number_of_steps))))),
                Domain::inRotatedQCone());

  M->constraint(Expr::vstack(tend, 0.5, Expr::sub(x->slice(nint(0,number_of_steps-1), nint(3,number_of_steps))->reshape(nint1(3)),
                                                  end_)), 
                Domain::inRotatedQCone());

  for (int i = 0; i < number_of_steps; i++)
    M->constraint(Expr::mul(A, x->slice(nint(0,i), nint(3,i+1))->reshape(nint1(3))), Domain::lessThan(b));

  M->setLogHandler([](const std::string& msg){std::cout<< msg << std::flush;});

  auto lambda1 = M->parameter("lambda_1");
  auto lambda2 = M->parameter("lambda_2");
  auto lambda3 = M->parameter("lambda_3");
  lambda1->setValue(lambda1_val);
  lambda2->setValue(lambda2_val);
  lambda3->setValue(lambda3_val);

  M->objective(ObjectiveSense::Minimize, 
               Expr::add( Expr::sum(Expr::mul(lambda2, t)),
                          Expr::add(Expr::mul(lambda1, tstart), Expr::mul(lambda3, tend))));

  M->writeTask("a.ptf");
  M->solve();
  auto sol = *(x->level());
  std::cout<< "solution "<< sol << std::endl;

}

Could not get the expected output

1 Answers1