-1

问题：

我不会得到从 Matlab 实现中得到的结果，我不确定pow_pos(norm(x(:, 1) - start'), 2)我是否正确转换，这是转换后的代码

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

输出

这里黑点代表我从 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 , 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

这是使用 Mosek 将上述内容转换为 c++ 的代码

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

}
4

1 回答 1

1

在@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},
                                      {-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;

}
于 2021-06-29T10:59:03.190 回答