Could not get the expected output - c++

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;
}

Answer 1

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;
}