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, const Eigen::VectorXd &h, const Eigen::VectorXd &primal)
22  {
23  Eigen::MatrixXd L = H.triangularView<Eigen::Lower>();
24 
25  double result = 0.5 * primal.transpose() * L * L.transpose() * primal;
26 
27  if (h.rows() > 0)
28  {
29  result += h.transpose() * primal;
30  }
31 
32  std::cout << "||| Objective = " << result << std::endl;
33 
34  return (result);
35  }
36 
37 
38  template <class t_ActiveSet, class t_ConstraintStatuses>
40  const Eigen::MatrixXd &H,
41  const Eigen::VectorXd &h,
42  const Eigen::VectorXd &primal,
43  const Eigen::MatrixXd &A,
44  const t_ActiveSet &active_set,
45  const MatrixIndex &num_simple_bounds,
46  const t_ConstraintStatuses &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 and active_set.num_equalities_ < active_set.size_)
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]================================="
104  << std::endl;
105  for (MatrixIndex i = 0; i < active_set.size_; ++i)
106  {
107  MatrixIndex ctr_index = active_set.getIndex(i);
108  std::cout << " " << i;
109  switch (constraints_status[ctr_index])
110  {
112  std::cout << "L ";
113  break;
115  std::cout << "U ";
116  break;
118  std::cout << "E ";
119  break;
120  default:
121  break;
122  }
123 
124  std::cout << "dual " << dual(i) << " | "
125  << "ref " << dual_check(i) << " | ";
126 
127 
128  switch (constraints_status[ctr_index])
129  {
132  std::cout << "err " << std::abs(dual(i) - dual_check(i)) << " | ";
133  if (dual_direction.rows() > 0)
134  {
135  std::cout << "dir " << dual_direction(i) << " | "
136  << "len " << dual(i) / dual_direction(i) << std::endl;
137  }
138  else
139  {
140  std::cout << std::endl;
141  }
142  if (max_diff < std::abs(dual(i) - dual_check(i)))
143  {
144  max_diff = std::abs(dual(i) - dual_check(i));
145  }
146  break;
147 
149  std::cout << std::endl;
150  break;
151 
152  default:
153  QPMAD_UTILS_THROW("This should not happen!");
154  break;
155  }
156  }
157  std::cout << " MAX DIFF = " << max_diff << std::endl;
158  std::cout << "================================================================================"
159  << std::endl;
160  }
161  }
162 
163 
164  template <class t_ActiveSet, class t_ConstraintStatuses>
166  const t_ActiveSet &active_set,
167  const t_ConstraintStatuses &constraints_status,
168  const Eigen::VectorXd &dual)
169  {
170  std::cout << "====================================[Active set]================================"
171  << std::endl;
172  for (MatrixIndex i = active_set.num_equalities_; i < active_set.size_; ++i)
173  {
174  MatrixIndex active_ctr_index = active_set.getIndex(i);
175 
176  std::cout << " ## " << i << " ## | Index = " << active_ctr_index
177  << " | Type = " << constraints_status[active_ctr_index] << " | Dual = " << dual(i)
178  << std::endl;
179  }
180  std::cout << "================================================================================"
181  << std::endl;
182  }
183 
184  template <class t_Dual, class t_Indices, class t_IsLower>
186  const t_Dual &dual,
187  const t_Indices &indices,
188  const t_IsLower &is_lower)
189  {
190  std::cout << "================================[Dual variables]================================"
191  << std::endl;
193  dual.size() == indices.size() and dual.size() == is_lower.size(), "Inconsistent vector length");
194  for (MatrixIndex i = 0; i < dual.size(); ++i)
195  {
196  std::cout << " ## " << i << " ## | Index = " << indices(i) << " | Lower = " << is_lower(i)
197  << " | Dual = " << dual(i) << std::endl;
198  }
199  std::cout << "================================================================================"
200  << std::endl;
201  }
202  } // namespace testing
203 } // namespace qpmad
#define QPMAD_UTILS_PERSISTENT_ASSERT(condition, message)
#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 t_ActiveSet &active_set, const t_ConstraintStatuses &constraints_status, const Eigen::VectorXd &dual)
Definition: testing.h:165
void checkLagrangeMultipliers(const Eigen::MatrixXd &H, const Eigen::VectorXd &h, const Eigen::VectorXd &primal, const Eigen::MatrixXd &A, const t_ActiveSet &active_set, const MatrixIndex &num_simple_bounds, const t_ConstraintStatuses &constraints_status, const Eigen::VectorXd &dual, const Eigen::VectorXd &dual_direction=Eigen::VectorXd())
Definition: testing.h:39
void printDualVariables(const t_Dual &dual, const t_Indices &indices, const t_IsLower &is_lower)
Definition: testing.h:185
EIGEN_DEFAULT_DENSE_INDEX_TYPE MatrixIndex
Definition: common.h:32