search

robotology / osqp-eigen

Simple Eigen-C++ wrapper for OSQP library
https://robotology.github.io/osqp-eigen/
BSD 3-Clause "New" or "Revised" License
393 stars 118 forks source link

undefined reference to `OsqpEigen::Solver::Solver()',undefined reference to `OsqpEigen::Solver::settings() const' #151

Open zhou-0423 opened 11 months ago

zhou-0423 commented 11 months ago

the system is windows10, osqp 0.6.3,osqp-eigen0.8.1. In s-function-builder of matlab, configure is (E:\C++\eigen\install1\include\eigen3;E:\C++\osqp\include;E:\C++\osqp-eigen\include). code is: / Includes_BEGIN /

include

include

include <Eigen/Dense>

include

include <OsqpEigen/OsqpEigen.h>

define PI acos(-1)

//幂计算函数 Eigen::MatrixXd multi_matrix(Eigen::MatrixXd Matrix_1, Eigen::MatrixXd Matrix_2, const int n) //两个方阵相乘，n为方阵维度 { Eigen::MatrixXd Matrix; Matrix.resize(Matrix_1.rows(), Matrix_1.cols()); Matrix = Eigen::MatrixXd::Zero(Matrix_1.rows(), Matrix_1.cols()); for (int i = 0; i < n; i++) { for (int j = 0; j < n; j++) { //Matrix(i,j) = 0; for (int k = 0; k < n; k++) { Matrix(i, j) += Matrix_1(i, k) * Matrix_2(k, j); } } }

return Matrix;

}

Eigen::MatrixXd calc_power(Eigen::MatrixXd Matrix, const int m, const int n) //计算次幂 ，m> 0; //m 为次方数 n为方阵维度 { if (m == 1) return Matrix; else { Eigen::MatrixXd Matrix_copy; //复制当前矩阵 Matrix_copy.resize(Matrix.rows(), Matrix.cols()); Matrix_copy = Matrix; // Matrix_copy = Eigen::MatrixXd::Zero(Matrix.rows(), Matrix.cols()); for (int i = 0; i < m - 1; i++) { Matrix = multi_matrix(Matrix, Matrix_copy, n); } return Matrix; } } //kron实现 Eigen::MatrixXd kron(Eigen::MatrixXd a,Eigen::MatrixXd b) { Eigen::MatrixXd p(a.rows()b.rows(),a.cols()b.cols()); for (int i=0;i<a.rows();i++) for (int j=0;j<a.cols();j++) for (int ii=0;ii<b.rows();ii++) for (int jj=0;jj<b.cols();jj++) { //行ib.row+ii，列jb.column+jj p(ib.rows()+ii,jb.cols()+jj)=a(i,j)b(ii,jj); } return p; } / Includes_END */

/ Externs_BEGIN / / extern double func(double a); / Eigen::Vector<double,Eigen::Dynamic> r; Eigen::Vector<double,Eigen::Dynamic> U;

double vd1 = 5.0;  // Assuming vd1 is a global variable
double vd2 = 0.104;  // Assuming vd2 is a global variable
double T = 0.05;   // 采样时间
double sample=0;
double heading_offset;
double Nx = 3;        // 状态量的个数
double Nu = 2;        // 控制量的个数
double Np = 80;       // 预测步长
double Nc = 30;       // 控制步长
double L = 2.910;  // 车辆的轴距

/ Externs_END /

void Modelprective_Start_wrapper(real_T xD) { / Start_BEGIN / /

void Modelprective_Outputs_wrapper(const real_T x_r, const real_T y_r, const real_T yaw_r, real_T vx, real_T steer, const real_T xD) { / Output_BEGIN / / 此示例将输出设置为等于输入 y0[0] = u0[0]; 对于复信号，使用: y0[0].re = u0[0].re; y0[0].im = u0[0].im; y1[0].re = u1[0].re; y1[0].im = u1[0].im; /

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> a;
a = Eigen::MatrixXd::Zero(3,3);
a << 1, 0, -vd1 * sin(r(2)) * T,
     0, 1, vd1 * cos(r(2)) * T,
     0, 0, 1;

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> b;
b = Eigen::MatrixXd::Zero(3,2);
b << cos(r(2)) * T, 0,
     sin(r(2)) * T, 0,
     tan(vd2) * T / L, vd1 * T / pow(cos(vd2), 2);

// 为了转化为标准二次规划问题，进行矩阵转化 // 状态和控制合并为一个新的向量 Eigen::Vector<double,Eigen::Dynamic> kesi; kesi = Eigen::VectorXd::Zero(5,1); kesi << xD[0], xD[1], xD[2], U(0), U(1);

heading_offset = *yaw_r - r(2);
if (heading_offset < (-PI))
{
    heading_offset = heading_offset+2*PI;
}
else if (heading_offset > PI)
{
    heading_offset = heading_offset - 2*PI;
}
else
{
    heading_offset = *yaw_r - r(2);
}
kesi(2)=heading_offset;
kesi(3)=U(0);
kesi(4)=U(1);

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A;
A = Eigen::MatrixXd::Zero(Nx + Nu, Nx + Nu);
A << a, b,
     Eigen::MatrixXd::Zero(Nu, Nx),
     Eigen::MatrixXd::Identity(Nu, Nu);

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> B;
B = Eigen::MatrixXd::Zero(Nx + Nu,  Nu);
B << b,
     Eigen::MatrixXd::Identity(Nu, Nu);

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> C;
C = Eigen::MatrixXd::Zero(Nx, Nx + Nu);
C << 1, 0, 0, 0, 0,
     0, 1, 0, 0, 0,
     0, 0, 1, 0, 0;

// 2.预测
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> phi_cell;
phi_cell = Eigen::MatrixXd::Zero(Np*Nx,Nx+Nu);

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> theta_cell;
theta_cell = Eigen::MatrixXd::Zero(Np*Nx,Nc*Nu);

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A_p;
A_p = Eigen::MatrixXd::Zero(Np+Nx,Nc+Nu);
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A_n;
A_n = Eigen::MatrixXd::Zero(Np+Nx,Nc+Nu);
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A_m;
for (int j = 0; j<Np; j++)
{
    A_n =calc_power(A,j+1,Nx + Nu);
    phi_cell.block(j*3,0,Nx,Nx+Nu)=C*(A_n);
    for (int k = 0; k<Nc; k++)
    {
        if (k<j)
        {
            A_p =calc_power(A,j-k,Nx + Nu);
            theta_cell.block(j*3,k*2,Nx,Nu)=C*A_p*B;
        }
        else if (k==j)
        {
            A_m = Eigen::MatrixXd::Identity(Np+Nx,Nc+Nu);
            theta_cell.block(j*3,k*2,Nx,Nu)=C*A_m*B;

        }
        else
            theta_cell <<  Eigen::MatrixXd::Identity(Np*Nx,Nc*Nu);

    }
}
// 其他变量声明和初始化...

// 3.目标函数
// 其他变量声明和初始化...
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Row;
Row = Eigen::MatrixXd::Zero(1,1);
Row << 10;

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Q;
Q = Eigen::MatrixXd::Identity(Nx*Np,Nx*Np);

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> R;
R = Eigen::MatrixXd::Identity(5,5);
R = R*5;

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> H;
H = Eigen::MatrixXd::Zero(2 * 30 + 1, 2 * 30 + 1);
H.block(0, 0, Nu * Nc, Nu * Nc) = theta_cell.transpose() * Q * theta_cell+ R;
H.block(0, Nu * Nc, Nu * Nc, 1).setZero();
H.block(Nu * Nc, 0, 1, Nu * Nc).setZero();
H.block(Nu * Nc, Nu * Nc,1,1) = Row;

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> error;
error = phi_cell*kesi;

// Eigen::VectorXd f(61);

Eigen::Vector<double,Eigen::Dynamic> f;
f = Eigen::VectorXd::Zero(2*30+1,1);
f.block(0,0,Nu * Nc,1) = error.transpose() * Q * theta_cell;
Eigen::MatrixXd value_zero(1,1);
value_zero << 0;
f.block(0,60,1,1) = value_zero;

// 4.约束
// 其他变量声明和初始化...
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A_t;
A_t = Eigen::MatrixXd::Zero(Nc,Nc);
for (int i=0; i<Nc; i++)
{
    for (int j=0; j<Nc; j++)
    {
        if (j<=i)
        {
            A_t(i,j)=1;
        }
        else
        {
            A_t(i,j)=0;
        }
    }
}

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> p1;
p1 = Eigen::MatrixXd::Zero(Nx,Nx);

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> p2;
p2 = Eigen::MatrixXd::Ones(Nc,1);

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> p3;
p2 = Eigen::MatrixXd::Zero(Nc*Nu,1);

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A_I;
A_I = kron(A_t,p1);
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Ut;
Ut = kron(p2,U);
//控制量约束
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> umin;
umin = Eigen::MatrixXd::Zero(2,1);
umin << -0.2,-0.436;

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> umax;
umax = Eigen::MatrixXd::Zero(2,1);
umax << 0.2,0.436;

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Umin;
Umin = kron(p2,umin);
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> Umax;
Umax = kron(p2,umax);

// Eigen::SparseMatrix A_cons(2230,230+1); Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A_cons; A_cons = Eigen::MatrixXd::Zero(2230,230+1); A_cons.block(0, 0, Nu Nc, Nu Nc)=A_I; A_cons.block(0, 60, Nu Nc, 1)=p3; A_cons.block(60, 0, Nu Nc, Nu Nc)=-A_I; A_cons.block(60, 60, Nu Nc, 1)=p3;

Eigen::Vector<double, Eigen::Dynamic> b_cons;
b_cons = Eigen::VectorXd::Zero(2*2*30,1);
b_cons.block(0, 0, Nu * Nc, 1)=Umax-Ut;
b_cons.block(60, 0, Nu * Nc, 1)= -Umax+Ut;
//控制增量和松弛因子约束

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> delta_umin;
delta_umin = Eigen::MatrixXd::Zero(2,1);
delta_umin << -0.05,-0.0082;

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> delta_umax;
delta_umax = Eigen::MatrixXd::Zero(2,1);
delta_umax << 0.05,0.0082;

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> delta_Umin;
delta_Umin = kron(p2,delta_umin);
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> delta_Umax;
delta_Umax = kron(p2,delta_umax);

// 6.输出

// Eigen::VectorXd lb(2); Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> lb; lb = Eigen::MatrixXd::Zero(2,1); lb << delta_Umin,0;

// Eigen::VectorXd ub(2); Eigen::Vector<double, Eigen::Dynamic> ub; ub = Eigen::VectorXd::Zero(2,1); ub << delta_Umax,10;

// Eigen::Vector<double, Eigen::Dynamic> X; // double solve_quadprog(Eigen::Matrix<double,61,61>& H, Eigen::Vector<double,61>& f,const Eigen::Matrix<double,120,61>& A_cons, const Eigen::Vector<double,120>& b_cons, const Eigen::Matrix<double,2,1>& lb, const Eigen::Vector<double,2>& ub, Eigen::Vector<double,2>& X);

Eigen::SparseMatrix<double> H_b(61,61);
Eigen::SparseMatrix<double> A_cons_b(2*2*30,2*30+1);
for (int i = 0;i<61;i++)
{
    for (int j=0;j<61;j++)
    {
        H_b.insert(i,j)=H(i,j);
    }

}
for (int i = 0;i<120;i++)
{
    for (int j=0;j<61;j++)
    {
        A_cons_b.insert(i,j)=A_cons(i,j);
    }

}

OsqpEigen::Solver solver;
solver.settings()->setVerbosity(false);
solver.settings()->setWarmStart(true);

// set the initial data of the QP solver
solver.data()->setNumberOfVariables(2);   //变量数n
solver.data()->setNumberOfConstraints(2); //约束数m
if (!solver.data()->setHessianMatrix(H_b))
    std::cout << 1;
if (!solver.data()->setGradient(f))
    std::cout << 1;
if (!solver.data()->setLinearConstraintsMatrix(A_cons_b))
    std::cout << 1;
if (!solver.data()->setLowerBound(lb))
    std::cout << 1;
if (!solver.data()->setUpperBound(ub))
   std::cout << 1;

// instantiate the solver
if (!solver.initSolver())
   std::cout << 1;

// solve the QP problem
if (solver.solveProblem() != OsqpEigen::ErrorExitFlag::NoError)
{
   std::cout << 1;
}

Eigen::VectorXd QPSolution;
QPSolution = solver.getSolution();

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> u_piao;
u_piao = Eigen::MatrixXd::Zero(Nu,1);

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> u_real;
u_real = Eigen::MatrixXd::Zero(2,1);
u_piao(0) =  QPSolution(0);
u_piao(1) =  QPSolution(1);
U(0) = kesi(3) + u_piao(0);
U(1) = kesi(4) + u_piao(1);
u_real(0) = U(0) + vd1;
u_real(1) = U(1) + vd2;
*vx = xD[0];
*steer = xD[1];

/ Output_END / }

void Modelprective_Update_wrapper(const real_T x_r, const real_T y_r, const real_T yaw_r, real_T vx, real_T steer, real_T xD) { / Update_BEGIN / sample = sample+T; r(0) = 25sin(0.2sample); r(1) = 35-25cos(0.2sample); r(2) = 0.2*sample;

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> a;
a = Eigen::MatrixXd::Zero(3,3);
a << 1, 0, -vd1 * sin(r(2)) * T,
     0, 1, vd1 * cos(r(2)) * T,
     0, 0, 1;

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> b;
b = Eigen::MatrixXd::Zero(3,2);
b << cos(r(2)) * T, 0,
     sin(r(2)) * T, 0,
     tan(vd2) * T / L, vd1 * T / pow(cos(vd2), 2);

Eigen::Vector<double,Eigen::Dynamic> ob;
ob = Eigen::VectorXd::Zero(3,1);
ob << *x_r - r(0),
        *y_r - r(1),
        *yaw_r - r(2);

Eigen::Vector<double,Eigen::Dynamic> tempX;
tempX = Eigen::VectorXd::Zero(3,1);
tempX << xD[0],
         xD[1],
         xD[2],
tempX = a*tempX+b*U;
xD[0] = tempX[0];
xD[1] = tempX[1];
xD[2] = tempX[2];

/ Update_END / }

void Modelprective_Terminate_wrapper(real_T xD) { / Terminate_BEGIN / /

S-Dafarra commented 10 months ago
  • E:/C++/osqp-eigen/include/OsqpEigen/SparseMatrixHelper.tpp:30: undefined reference to OsqpEigen::debugStream()' E:/C++/osqp-eigen/include/OsqpEigen/SparseMatrixHelper.tpp:35: undefined reference tocsc_spalloc'

I think this can be related to https://github.com/osqp/osqp/issues/576