13 #include <Eigen/Dense> 22 const Eigen::VectorXd &h,
23 const Eigen::VectorXd &primal)
25 Eigen::MatrixXd L = H.triangularView<Eigen::Lower>();
27 double result = 0.5 * primal.transpose() * L * L.transpose() * primal;
31 result += h.transpose() * primal;
34 std::cout <<
"||| Objective = " << result << std::endl;
41 const Eigen::VectorXd &h,
42 const Eigen::VectorXd &primal,
43 const Eigen::MatrixXd &A,
46 const std::vector<ConstraintStatus::Status> & constraints_status,
47 const Eigen::VectorXd & dual,
48 const Eigen::VectorXd & dual_direction = Eigen::VectorXd())
50 Eigen::MatrixXd L = H.triangularView<Eigen::Lower>();
51 Eigen::VectorXd v = L * L.transpose() * primal;
59 M.resize(primal.rows(), active_set.
size_);
65 if (ctr_index < num_simple_bounds)
68 switch(constraints_status[ctr_index])
71 M(ctr_index, i) = -1.0;
75 M(ctr_index, i) = 1.0;
83 switch(constraints_status[ctr_index])
86 M.col(i) = -A.row(ctr_index-num_simple_bounds).transpose();
90 M.col(i) = A.row(ctr_index-num_simple_bounds).transpose();
99 Eigen::HouseholderQR<Eigen::MatrixXd> dec(M);
100 Eigen::VectorXd dual_check = dec.solve(-v);
102 double max_diff = 0.0;
103 std::cout <<
"===============================[Dual variables]=================================" << std::endl;
107 std::cout <<
" " << i;
108 switch(constraints_status[ctr_index])
123 std::cout <<
"dual " << dual(i) <<
" | " 124 <<
"ref " << dual_check(i) <<
" | ";
127 switch(constraints_status[ctr_index])
131 std::cout <<
"err " << std::abs(dual(i) - dual_check(i)) <<
" | " ;
132 if (dual_direction.rows() > 0)
134 std::cout <<
"dir " << dual_direction(i) <<
" | " 135 <<
"len " << dual(i) / dual_direction(i) << std::endl;
139 std::cout << std::endl;
141 if (max_diff < std::abs(dual(i) - dual_check(i)))
143 max_diff = std::abs(dual(i) - dual_check(i));
148 std::cout << std::endl;
156 std::cout <<
" MAX DIFF = " << max_diff << std::endl;
157 std::cout <<
"================================================================================" << std::endl;
163 const std::vector<ConstraintStatus::Status> & constraints_status,
164 const Eigen::VectorXd & dual)
166 std::cout <<
"====================================[Active set]================================" << std::endl;
171 std::cout <<
" ## " << i
172 <<
" ## | Index = " << active_ctr_index
173 <<
" | Type = " << constraints_status[active_ctr_index]
174 <<
" | Dual = " << dual(i)
177 std::cout <<
"================================================================================" << std::endl;
#define QPMAD_UTILS_THROW(s)
double computeObjective(const Eigen::MatrixXd &H, const Eigen::VectorXd &h, const Eigen::VectorXd &primal)
void printActiveSet(const ActiveSet &active_set, const std::vector< ConstraintStatus::Status > &constraints_status, const Eigen::VectorXd &dual)
void checkLagrangeMultipliers(const Eigen::MatrixXd &H, const Eigen::VectorXd &h, const Eigen::VectorXd &primal, const Eigen::MatrixXd &A, const ActiveSet &active_set, const MatrixIndex &num_simple_bounds, const std::vector< ConstraintStatus::Status > &constraints_status, const Eigen::VectorXd &dual, const Eigen::VectorXd &dual_direction=Eigen::VectorXd())
MatrixIndex getIndex(const MatrixIndex index) const
MatrixIndex num_equalities_