-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
integer_optimization_util_test.cc
151 lines (135 loc) · 5.31 KB
/
integer_optimization_util_test.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
#include "drake/solvers/integer_optimization_util.h"
#include <gtest/gtest.h>
#include "drake/solvers/mathematical_program.h"
#include "drake/solvers/osqp_solver.h"
#include "drake/solvers/solve.h"
namespace drake {
namespace solvers {
namespace {
class IntegerOptimizationUtilTest : public ::testing::Test {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(IntegerOptimizationUtilTest)
enum class LogicalOperand { kAnd, kOr, kXor };
IntegerOptimizationUtilTest()
: prog_{},
b_{prog_.NewContinuousVariables<2>()},
b_vals_{{Eigen::Vector2i(0, 0), Eigen::Vector2i(0, 1),
Eigen::Vector2i(1, 0), Eigen::Vector2i(1, 1)}},
b0_cnstr_{prog_.AddLinearConstraint(b_(0) == 0)},
b1_cnstr_{prog_.AddLinearConstraint(b_(1) == 0)} {}
void CheckLogicalOperand(LogicalOperand operand) {
// We want to make sure that given that b0 and b1 are fixed, the only
// solution to satisfying the constraints, is that
// operand_result = b0 operand b1.
symbolic::Variable operand_result = prog_.NewContinuousVariables<1>()(0);
switch (operand) {
case LogicalOperand::kAnd:
prog_.AddConstraint(CreateLogicalAndConstraint(
symbolic::Expression{b_(0)}, symbolic::Expression{b_(1)},
symbolic::Expression{operand_result}));
break;
case LogicalOperand::kOr:
prog_.AddConstraint(CreateLogicalOrConstraint(
symbolic::Expression{b_(0)}, symbolic::Expression{b_(1)},
symbolic::Expression{operand_result}));
break;
case LogicalOperand::kXor:
prog_.AddConstraint(CreateLogicalXorConstraint(
symbolic::Expression{b_(0)}, symbolic::Expression{b_(1)},
symbolic::Expression{operand_result}));
break;
}
auto cost = prog_.AddLinearCost(operand_result);
// Solve the program
// min operand_result
// s.t operand_result = b0 operand b1
// b0 = b_vals_[i](0)
// b1 = b_vals_[i](1).
for (int i = 0; i < 4; ++i) {
double operand_result_expected;
switch (operand) {
case LogicalOperand::kAnd:
operand_result_expected =
static_cast<double>(b_vals_[i](0) & b_vals_[i](1));
break;
case LogicalOperand::kOr:
operand_result_expected =
static_cast<double>(b_vals_[i](0) | b_vals_[i](1));
break;
case LogicalOperand::kXor:
operand_result_expected =
static_cast<double>(b_vals_[i](0) ^ b_vals_[i](1));
break;
}
cost.evaluator()->UpdateCoefficients(Vector1d(1));
b0_cnstr_.evaluator()->UpdateLowerBound(Vector1d(b_vals_[i](0)));
b0_cnstr_.evaluator()->UpdateUpperBound(Vector1d(b_vals_[i](0)));
b1_cnstr_.evaluator()->UpdateLowerBound(Vector1d(b_vals_[i](1)));
b1_cnstr_.evaluator()->UpdateUpperBound(Vector1d(b_vals_[i](1)));
MathematicalProgramResult result = Solve(prog_);
EXPECT_TRUE(result.is_success());
double tol = 1E-3;
if (result.get_solver_id() == OsqpSolver::id()) {
// OSQP can solve linear program, but it is likely to fail in polishing,
// thus the solution is less accurate.
tol = 3E-3;
}
EXPECT_NEAR(result.GetSolution(operand_result), operand_result_expected,
tol);
// Now update the objective to
// min -b_and
// The solution should be the same.
cost.evaluator()->UpdateCoefficients(Vector1d(-1));
result = Solve(prog_);
EXPECT_TRUE(result.is_success());
EXPECT_NEAR(result.GetSolution(operand_result), operand_result_expected,
tol);
}
}
protected:
MathematicalProgram prog_;
VectorDecisionVariable<2> b_;
std::array<Eigen::Vector2i, 4> b_vals_;
Binding<LinearConstraint> b0_cnstr_;
Binding<LinearConstraint> b1_cnstr_;
};
TEST_F(IntegerOptimizationUtilTest, TestAnd) {
CheckLogicalOperand(LogicalOperand::kAnd);
}
TEST_F(IntegerOptimizationUtilTest, TestOr) {
CheckLogicalOperand(LogicalOperand::kOr);
}
TEST_F(IntegerOptimizationUtilTest, TestXor) {
CheckLogicalOperand(LogicalOperand::kXor);
}
GTEST_TEST(TestBinaryCodeMatchConstraint, Test) {
MathematicalProgram prog;
const auto b = prog.NewContinuousVariables<3>();
auto b_constraint = prog.AddBoundingBoxConstraint(0, 1, b);
Eigen::Vector3i b_expected{0, 1, 1};
const auto match = prog.NewContinuousVariables<1>()(0);
prog.AddConstraint(CreateBinaryCodeMatchConstraint(b, b_expected, match));
auto match_constraint = prog.AddBoundingBoxConstraint(0, 1, match);
for (int b0_val : {0, 1}) {
for (int b1_val : {0, 1}) {
for (int b2_val : {0, 1}) {
for (double match_val : {-0.5, 0.0, 0.5, 1.0}) {
const Eigen::Vector3d b_val(b0_val, b1_val, b2_val);
b_constraint.evaluator()->UpdateLowerBound(b_val);
b_constraint.evaluator()->UpdateUpperBound(b_val);
match_constraint.evaluator()->UpdateUpperBound(Vector1d(match_val));
match_constraint.evaluator()->UpdateLowerBound(Vector1d(match_val));
const auto result = Solve(prog);
if (b0_val == 0 && b1_val == 1 && b2_val == 1) {
EXPECT_EQ(result.is_success(), match_val == 1.0);
} else {
EXPECT_EQ(result.is_success(), match_val == 0.0);
}
}
}
}
}
}
} // namespace
} // namespace solvers
} // namespace drake