forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
/
mixed_integer_optimization_util.cc
179 lines (168 loc) · 7.05 KB
/
mixed_integer_optimization_util.cc
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
#include "drake/solvers/mixed_integer_optimization_util.h"
#include "drake/math/gray_code.h"
#include "drake/solvers/integer_optimization_util.h"
using drake::symbolic::Expression;
namespace drake {
namespace solvers {
std::string to_string(IntervalBinning binning) {
switch (binning) {
case IntervalBinning::kLinear: {
return "linear_binning";
}
case IntervalBinning::kLogarithmic: {
return "logarithmic_binning";
}
}
DRAKE_UNREACHABLE();
}
std::ostream& operator<<(std::ostream& os, const IntervalBinning& binning) {
os << to_string(binning);
return os;
}
void AddLogarithmicSos2Constraint(
MathematicalProgram* prog,
const Eigen::Ref<const VectorX<symbolic::Expression>>& lambda,
const Eigen::Ref<const VectorX<symbolic::Expression>>& y) {
const int num_lambda = lambda.rows();
for (int i = 0; i < num_lambda; ++i) {
prog->AddLinearConstraint(lambda(i) >= 0);
prog->AddLinearConstraint(lambda(i) <= 1);
}
prog->AddLinearConstraint(lambda.sum() == 1);
const int num_interval = num_lambda - 1;
const int num_binary_vars = CeilLog2(num_interval);
DRAKE_DEMAND(y.rows() == num_binary_vars);
const auto gray_codes = math::CalculateReflectedGrayCodes(num_binary_vars);
DRAKE_ASSERT(y.rows() == num_binary_vars);
for (int j = 0; j < num_binary_vars; ++j) {
symbolic::Expression lambda_sum1 = gray_codes(0, j) == 1 ? lambda(0) : 0;
symbolic::Expression lambda_sum2 = gray_codes(0, j) == 0 ? lambda(0) : 0;
for (int i = 1; i < num_lambda - 1; ++i) {
lambda_sum1 +=
(gray_codes(i - 1, j) == 1 && gray_codes(i, j) == 1) ? lambda(i) : 0;
lambda_sum2 +=
(gray_codes(i - 1, j) == 0 && gray_codes(i, j) == 0) ? lambda(i) : 0;
}
lambda_sum1 +=
gray_codes(num_lambda - 2, j) == 1 ? lambda(num_lambda - 1) : 0;
lambda_sum2 +=
gray_codes(num_lambda - 2, j) == 0 ? lambda(num_lambda - 1) : 0;
prog->AddLinearConstraint(lambda_sum1 <= y(j));
prog->AddLinearConstraint(lambda_sum2 <= 1 - y(j));
}
}
void AddSos2Constraint(
MathematicalProgram* prog,
const Eigen::Ref<const VectorX<symbolic::Expression>>& lambda,
const Eigen::Ref<const VectorX<symbolic::Expression>>& y) {
if (lambda.rows() != y.rows() + 1) {
throw std::runtime_error(
"The size of y and lambda do not match when adding the SOS2 "
"constraint.");
}
prog->AddLinearConstraint(lambda.sum() == 1);
prog->AddLinearConstraint(lambda(0) <= y(0) && lambda(0) >= 0);
for (int i = 1; i < y.rows(); ++i) {
prog->AddLinearConstraint(lambda(i) <= y(i - 1) + y(i) && lambda(i) >= 0);
}
prog->AddLinearConstraint(lambda.tail<1>()(0) >= 0 &&
lambda.tail<1>()(0) <= y.tail<1>()(0));
prog->AddLinearConstraint(y.sum() == 1);
}
void AddLogarithmicSos1Constraint(
MathematicalProgram* prog,
const Eigen::Ref<const VectorX<symbolic::Expression>>& lambda,
const Eigen::Ref<const VectorXDecisionVariable>& y,
const Eigen::Ref<const Eigen::MatrixXi>& codes) {
const int num_lambda = lambda.rows();
const int num_digits = CeilLog2(num_lambda);
DRAKE_DEMAND(codes.rows() == num_lambda && codes.cols() == num_digits);
DRAKE_DEMAND(y.rows() == num_digits);
for (int i = 0; i < num_digits; ++i) {
DRAKE_ASSERT(y(i).get_type() == symbolic::Variable::Type::BINARY);
}
for (int i = 0; i < num_lambda; ++i) {
prog->AddLinearConstraint(lambda(i) >= 0);
}
prog->AddLinearConstraint(lambda.sum() == 1);
for (int j = 0; j < num_digits; ++j) {
symbolic::Expression lambda_sum1{0};
symbolic::Expression lambda_sum2{0};
for (int k = 0; k < num_lambda; ++k) {
if (codes(k, j) == 1) {
lambda_sum1 += lambda(k);
} else if (codes(k, j) == 0) {
lambda_sum2 += lambda(k);
} else {
throw std::runtime_error("The codes entry can be only 0 or 1.");
}
}
prog->AddLinearConstraint(lambda_sum1 <= y(j));
prog->AddLinearConstraint(lambda_sum2 <= 1 - y(j));
}
}
void AddBilinearProductMcCormickEnvelopeMultipleChoice(
MathematicalProgram* prog, const symbolic::Variable& x,
const symbolic::Variable& y, const symbolic::Expression& w,
const Eigen::Ref<const Eigen::VectorXd>& phi_x,
const Eigen::Ref<const Eigen::VectorXd>& phi_y,
const Eigen::Ref<const VectorX<symbolic::Expression>>& Bx,
const Eigen::Ref<const VectorX<symbolic::Expression>>& By) {
const int m = phi_x.rows() - 1;
const int n = phi_y.rows() - 1;
DRAKE_ASSERT(Bx.rows() == m);
DRAKE_ASSERT(By.rows() == n);
const auto x_bar = prog->NewContinuousVariables(n, x.get_name() + "_x_bar");
const auto y_bar = prog->NewContinuousVariables(m, y.get_name() + "_y_bar");
prog->AddLinearEqualityConstraint(x_bar.cast<Expression>().sum() - x, 0);
prog->AddLinearEqualityConstraint(y_bar.cast<Expression>().sum() - y, 0);
const auto Bxy = prog->NewContinuousVariables(
m, n, x.get_name() + "_" + y.get_name() + "_Bxy");
prog->AddLinearEqualityConstraint(
Bxy.cast<Expression>().rowwise().sum().sum(), 1);
for (int i = 0; i < m; ++i) {
prog->AddLinearConstraint(
y_bar(i) >=
phi_y.head(n).dot(Bxy.row(i).transpose().cast<Expression>()));
prog->AddLinearConstraint(
y_bar(i) <=
phi_y.tail(n).dot(Bxy.row(i).transpose().cast<Expression>()));
}
for (int j = 0; j < n; ++j) {
prog->AddLinearConstraint(x_bar(j) >=
phi_x.head(m).dot(Bxy.col(j).cast<Expression>()));
prog->AddLinearConstraint(x_bar(j) <=
phi_x.tail(m).dot(Bxy.col(j).cast<Expression>()));
}
// The right-hand side of the constraint on w, we will implement
// w >= w_constraint_rhs(0)
// w >= w_constraint_rhs(1)
// w <= w_constraint_rhs(2)
// w <= w_constraint_rhs(3)
Vector4<symbolic::Expression> w_constraint_rhs(0, 0, 0, 0);
w_constraint_rhs(0) = x_bar.cast<Expression>().dot(phi_y.head(n)) +
y_bar.cast<Expression>().dot(phi_x.head(m));
w_constraint_rhs(1) = x_bar.cast<Expression>().dot(phi_y.tail(n)) +
y_bar.cast<Expression>().dot(phi_x.tail(m));
w_constraint_rhs(2) = x_bar.cast<Expression>().dot(phi_y.head(n)) +
y_bar.cast<Expression>().dot(phi_x.tail(m));
w_constraint_rhs(3) = x_bar.cast<Expression>().dot(phi_y.tail(n)) +
y_bar.cast<Expression>().dot(phi_x.head(m));
for (int i = 0; i < m; ++i) {
for (int j = 0; j < n; ++j) {
prog->AddConstraint(
// +v converts a symbolic variable v to a symbolic expression.
CreateLogicalAndConstraint(+Bx(i), +By(j), +Bxy(i, j)));
w_constraint_rhs(0) -= phi_x(i) * phi_y(j) * Bxy(i, j);
w_constraint_rhs(1) -= phi_x(i + 1) * phi_y(j + 1) * Bxy(i, j);
w_constraint_rhs(2) -= phi_x(i + 1) * phi_y(j) * Bxy(i, j);
w_constraint_rhs(3) -= phi_x(i) * phi_y(j + 1) * Bxy(i, j);
}
}
prog->AddLinearConstraint(w >= w_constraint_rhs(0));
prog->AddLinearConstraint(w >= w_constraint_rhs(1));
prog->AddLinearConstraint(w <= w_constraint_rhs(2));
prog->AddLinearConstraint(w <= w_constraint_rhs(3));
}
} // namespace solvers
} // namespace drake