1 |
|
|
// |
2 |
|
|
// Copyright (c) 2017 CNRS |
3 |
|
|
// |
4 |
|
|
// This file is part of tsid |
5 |
|
|
// tsid is free software: you can redistribute it |
6 |
|
|
// and/or modify it under the terms of the GNU Lesser General Public |
7 |
|
|
// License as published by the Free Software Foundation, either version |
8 |
|
|
// 3 of the License, or (at your option) any later version. |
9 |
|
|
// tsid is distributed in the hope that it will be |
10 |
|
|
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
11 |
|
|
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
12 |
|
|
// General Lesser Public License for more details. You should have |
13 |
|
|
// received a copy of the GNU Lesser General Public License along with |
14 |
|
|
// tsid If not, see |
15 |
|
|
// <http://www.gnu.org/licenses/>. |
16 |
|
|
// |
17 |
|
|
|
18 |
|
|
#include "tsid/solvers/utils.hpp" |
19 |
|
|
#include "tsid/math/constraint-base.hpp" |
20 |
|
|
|
21 |
|
|
#include <iostream> |
22 |
|
|
|
23 |
|
|
namespace tsid { |
24 |
|
|
namespace solvers { |
25 |
|
|
|
26 |
|
|
std::string HQPDataToString(const HQPData& data, bool printMatrices) { |
27 |
|
|
using namespace std; |
28 |
|
|
|
29 |
|
|
stringstream ss; |
30 |
|
|
unsigned int priority = 0; |
31 |
|
|
for (HQPData::const_iterator it = data.begin(); it != data.end(); it++) { |
32 |
|
|
ss << "Level " << priority << endl; |
33 |
|
|
for (ConstraintLevel::const_iterator iit = it->begin(); iit != it->end(); |
34 |
|
|
iit++) { |
35 |
|
|
auto c = iit->second; |
36 |
|
|
ss << " - " << c->name() << ": w=" << iit->first << ", "; |
37 |
|
|
if (c->isEquality()) |
38 |
|
|
ss << "equality, "; |
39 |
|
|
else if (c->isInequality()) |
40 |
|
|
ss << "inequality, "; |
41 |
|
|
else |
42 |
|
|
ss << "bound, "; |
43 |
|
|
ss << c->rows() << "x" << c->cols() << endl; |
44 |
|
|
} |
45 |
|
|
priority++; |
46 |
|
|
} |
47 |
|
|
|
48 |
|
|
if (printMatrices) { |
49 |
|
|
ss << endl; |
50 |
|
|
for (HQPData::const_iterator it = data.begin(); it != data.end(); it++) { |
51 |
|
|
for (ConstraintLevel::const_iterator iit = it->begin(); iit != it->end(); |
52 |
|
|
iit++) { |
53 |
|
|
auto c = iit->second; |
54 |
|
|
ss << "*** " << c->name() << " *** "; |
55 |
|
|
if (c->isEquality()) { |
56 |
|
|
ss << "(equality)" << endl; |
57 |
|
|
ss << "A =\n" << c->matrix() << endl; |
58 |
|
|
ss << "b = " << c->vector().transpose() << endl; |
59 |
|
|
} else if (c->isInequality()) { |
60 |
|
|
ss << "(inequality)" << endl; |
61 |
|
|
ss << "A =\n" << c->matrix() << endl; |
62 |
|
|
ss << "lb = " << c->lowerBound().transpose() << endl; |
63 |
|
|
ss << "ub = " << c->upperBound().transpose() << endl; |
64 |
|
|
} else { |
65 |
|
|
ss << "(bounds)" << endl; |
66 |
|
|
ss << "lb = " << c->lowerBound().transpose() << endl; |
67 |
|
|
ss << "ub = " << c->upperBound().transpose() << endl; |
68 |
|
|
} |
69 |
|
|
ss << endl; |
70 |
|
|
} |
71 |
|
|
} |
72 |
|
|
} |
73 |
|
|
return ss.str(); |
74 |
|
|
} |
75 |
|
|
|
76 |
|
|
} // namespace solvers |
77 |
|
|
} // namespace tsid |