问题描述
问题:
我不会得到我从 Matlab 实现中得到的结果,我不确定 pow_pos(norm(x(:,1) - start'),2)
我是否正确转换,这是转换后的代码
Variable::t x_0 = Var::vstack(x->index(0,0),x->index(1,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));
这里黑点代表我从 Matlab 得到的,绿点代表我从 C++ 得到的
这是我在 Matlab 中编写的原始代码,它给出了预期的输出
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,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(:,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
#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.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});
auto start_ = new_array_ptr<double,1>({-4,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));
M->constraint(Expr::vstack(t_tmp->index(0),Domain::inQCone());
M->constraint(Expr::hstack(t->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),i));
Variable::t x_i1 = Var::vstack(x->index(0,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),t_tmp->index(i)),Domain::inPPowerCone(1.0/2));
}
Variable::t x_n = Var::vstack(x->index(0,number_of_steps-1),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),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));
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;
}
解决方法
在@michal-adamaszek 的帮助下以及 Mosek Google Group (https://mail.google.com/mail/u/0/#inbox/FMfcgzGkXmgmHqFBVJFSNRWmjQfQSPlg) 中给出的答案以下是上述问题的有效解决方案,
#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.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,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,number_of_steps-1)),x->slice(nint(0,1),number_of_steps))))),Domain::inRotatedQCone());
M->constraint(Expr::vstack(tend,number_of_steps-1),number_of_steps))->reshape(nint1(3)),end_)),Domain::inRotatedQCone());
for (int i = 0; i < number_of_steps; i++)
M->constraint(Expr::mul(A,i),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;
}