qpmad
Eigen-based C++ QP solver.
testing.h
Go to the documentation of this file.
1 /**
2  @file
3  @author Alexander Sherikov
4 
5  @copyright 2017 Alexander Sherikov. Licensed under the Apache License,
6  Version 2.0. (see LICENSE or http://www.apache.org/licenses/LICENSE-2.0)
7 
8  @brief
9 */
10 
11 #pragma once
12 
13 #include <Eigen/Dense>
14 #include <iostream>
15 #include <iomanip>
16 
17 namespace qpmad
18 {
19  namespace testing
20  {
21  double computeObjective(const Eigen::MatrixXd &H,
22  const Eigen::VectorXd &h,
23  const Eigen::VectorXd &primal)
24  {
25  Eigen::MatrixXd L = H.triangularView<Eigen::Lower>();
26 
27  double result = 0.5 * primal.transpose() * L * L.transpose() * primal;
28 
29  if (h.rows() > 0)
30  {
31  result += h.transpose() * primal;
32  }
33 
34  std::cout << "||| Objective = " << result << std::endl;
35 
36  return(result);
37  }
38 
39 
40  void checkLagrangeMultipliers( const Eigen::MatrixXd &H,
41  const Eigen::VectorXd &h,
42  const Eigen::VectorXd &primal,
43  const Eigen::MatrixXd &A,
44  const ActiveSet &active_set,
45  const MatrixIndex &num_simple_bounds,
46  const std::vector<ConstraintStatus::Status> & constraints_status,
47  const Eigen::VectorXd & dual,
48  const Eigen::VectorXd & dual_direction = Eigen::VectorXd())
49  {
50  Eigen::MatrixXd L = H.triangularView<Eigen::Lower>();
51  Eigen::VectorXd v = L * L.transpose() * primal;
52  Eigen::MatrixXd M;
53 
54  if (h.rows() > 0)
55  {
56  v += h;
57  }
58 
59  M.resize(primal.rows(), active_set.size_);
60 
61  for (MatrixIndex i = 0; i < active_set.size_; ++i)
62  {
63  MatrixIndex ctr_index = active_set.getIndex(i);
64 
65  if (ctr_index < num_simple_bounds)
66  {
67  M.col(i).setZero();
68  switch(constraints_status[ctr_index])
69  {
71  M(ctr_index, i) = -1.0;
72  break;
75  M(ctr_index, i) = 1.0;
76  break;
77  default:
78  break;
79  }
80  }
81  else
82  {
83  switch(constraints_status[ctr_index])
84  {
86  M.col(i) = -A.row(ctr_index-num_simple_bounds).transpose();
87  break;
90  M.col(i) = A.row(ctr_index-num_simple_bounds).transpose();
91  break;
92  default:
93  break;
94  }
95  }
96  }
97  if (M.cols() > 0)
98  {
99  Eigen::HouseholderQR<Eigen::MatrixXd> dec(M);
100  Eigen::VectorXd dual_check = dec.solve(-v);
101 
102  double max_diff = 0.0;
103  std::cout << "===============================[Dual variables]=================================" << std::endl;
104  for (MatrixIndex i = 0; i < active_set.size_; ++i)
105  {
106  MatrixIndex ctr_index = active_set.getIndex(i);
107  std::cout << " " << i;
108  switch(constraints_status[ctr_index])
109  {
111  std::cout << "L ";
112  break;
114  std::cout << "U ";
115  break;
117  std::cout << "E ";
118  break;
119  default:
120  break;
121  }
122 
123  std::cout << "dual " << dual(i) << " | "
124  << "ref " << dual_check(i) << " | ";
125 
126 
127  switch(constraints_status[ctr_index])
128  {
131  std::cout << "err " << std::abs(dual(i) - dual_check(i)) << " | " ;
132  if (dual_direction.rows() > 0)
133  {
134  std::cout << "dir " << dual_direction(i) << " | "
135  << "len " << dual(i) / dual_direction(i) << std::endl;
136  }
137  else
138  {
139  std::cout << std::endl;
140  }
141  if (max_diff < std::abs(dual(i) - dual_check(i)))
142  {
143  max_diff = std::abs(dual(i) - dual_check(i));
144  }
145  break;
146 
148  std::cout << std::endl;
149  break;
150 
151  default:
152  QPMAD_UTILS_THROW("This should not happen!");
153  break;
154  }
155  }
156  std::cout << " MAX DIFF = " << max_diff << std::endl;
157  std::cout << "================================================================================" << std::endl;
158  }
159  }
160 
161 
162  void printActiveSet(const ActiveSet & active_set,
163  const std::vector<ConstraintStatus::Status> & constraints_status,
164  const Eigen::VectorXd & dual)
165  {
166  std::cout << "====================================[Active set]================================" << std::endl;
167  for (MatrixIndex i = active_set.num_equalities_; i < active_set.size_; ++i)
168  {
169  MatrixIndex active_ctr_index = active_set.getIndex(i);
170 
171  std::cout << " ## " << i
172  << " ## | Index = " << active_ctr_index
173  << " | Type = " << constraints_status[active_ctr_index]
174  << " | Dual = " << dual(i)
175  << std::endl;
176  }
177  std::cout << "================================================================================" << std::endl;
178  }
179  }
180 }
#define QPMAD_UTILS_THROW(s)
double computeObjective(const Eigen::MatrixXd &H, const Eigen::VectorXd &h, const Eigen::VectorXd &primal)
Definition: testing.h:21
void printActiveSet(const ActiveSet &active_set, const std::vector< ConstraintStatus::Status > &constraints_status, const Eigen::VectorXd &dual)
Definition: testing.h:162
int MatrixIndex
Definition: common.h:32
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())
Definition: testing.h:40
MatrixIndex getIndex(const MatrixIndex index) const
Definition: active_set.h:35
MatrixIndex num_equalities_
Definition: active_set.h:21
MatrixIndex size_
Definition: active_set.h:20