forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
622 lines
23 KiB
622 lines
23 KiB
#include "drake/solvers/mixed_integer_optimization_util.h"
|
|
|
|
#include <gtest/gtest.h>
|
|
|
|
#include "drake/common/test_utilities/eigen_matrix_compare.h"
|
|
#include "drake/math/gray_code.h"
|
|
#include "drake/solvers/gurobi_solver.h"
|
|
|
|
namespace drake {
|
|
namespace solvers {
|
|
namespace {
|
|
GTEST_TEST(TestMixedIntegerUtil, TestCeilLog2) {
|
|
// Check that CeilLog2(j) returns i + 1 for all j = 2ⁱ + 1, 2ⁱ + 2, ... , 2ⁱ⁺¹
|
|
const int kMaxExponent = 15;
|
|
EXPECT_EQ(0, CeilLog2(1));
|
|
for (int i = 0; i < kMaxExponent; ++i) {
|
|
for (int j = (1 << i) + 1; j <= (1 << (i + 1)); ++j) {
|
|
EXPECT_EQ(i + 1, CeilLog2(j));
|
|
}
|
|
}
|
|
}
|
|
|
|
GTEST_TEST(TestLogarithmicSos2, TestAddSos2) {
|
|
MathematicalProgram prog;
|
|
auto lambda1 = prog.NewContinuousVariables(3, "lambda1");
|
|
auto y1 =
|
|
AddLogarithmicSos2Constraint(&prog, lambda1.cast<symbolic::Expression>());
|
|
static_assert(std::is_same_v<decltype(y1), VectorXDecisionVariable>,
|
|
"y1 should be a dynamic-sized vector.");
|
|
auto lambda2 = prog.NewContinuousVariables<3>("lambda2");
|
|
auto y2 =
|
|
AddLogarithmicSos2Constraint(&prog, lambda2.cast<symbolic::Expression>());
|
|
static_assert(std::is_same_v<decltype(y2), VectorDecisionVariable<1>>,
|
|
"y2 should be a static-sized vector.");
|
|
}
|
|
|
|
void LogarithmicSos2Test(int num_lambda, bool logarithmic_binning) {
|
|
// Solve the program
|
|
// min λᵀ * λ
|
|
// s.t sum λ = 1
|
|
// λ in sos2
|
|
// We loop over i such that only λ(i) and λ(i+1) can be strictly positive.
|
|
// The optimal cost is λ(i) = λ(i + 1) = 0.5.
|
|
MathematicalProgram prog;
|
|
auto lambda = prog.NewContinuousVariables(num_lambda, "lambda");
|
|
prog.AddCost(lambda.cast<symbolic::Expression>().dot(lambda));
|
|
VectorXDecisionVariable y;
|
|
if (logarithmic_binning) {
|
|
y = AddLogarithmicSos2Constraint(&prog,
|
|
lambda.cast<symbolic::Expression>());
|
|
} else {
|
|
y = prog.NewBinaryVariables(num_lambda - 1);
|
|
AddSos2Constraint(&prog, lambda.cast<symbolic::Expression>(),
|
|
y.cast<symbolic::Expression>());
|
|
}
|
|
int num_binary_vars = y.rows();
|
|
int num_intervals = num_lambda - 1;
|
|
auto y_assignment = prog.AddBoundingBoxConstraint(0, 1, y);
|
|
|
|
// If we use logarithmic binning, we will assign the binary variables y with
|
|
// value i, expressed in Gray code.
|
|
const auto gray_codes = math::CalculateReflectedGrayCodes(num_binary_vars);
|
|
Eigen::VectorXd y_val(y.rows());
|
|
for (int i = 0; i < num_intervals; ++i) {
|
|
y_val.setZero();
|
|
if (logarithmic_binning) {
|
|
for (int j = 0; j < num_binary_vars; ++j) {
|
|
y_val(j) = gray_codes(i, j);
|
|
}
|
|
} else {
|
|
y_val(i) = 1;
|
|
}
|
|
y_assignment.evaluator()->UpdateLowerBound(y_val);
|
|
y_assignment.evaluator()->UpdateUpperBound(y_val);
|
|
|
|
GurobiSolver gurobi_solver;
|
|
if (gurobi_solver.available()) {
|
|
auto result = gurobi_solver.Solve(prog, {}, {});
|
|
EXPECT_TRUE(result.is_success());
|
|
const auto lambda_val = result.GetSolution(lambda);
|
|
Eigen::VectorXd lambda_val_expected = Eigen::VectorXd::Zero(num_lambda);
|
|
lambda_val_expected(i) = 0.5;
|
|
lambda_val_expected(i + 1) = 0.5;
|
|
EXPECT_TRUE(CompareMatrices(lambda_val, lambda_val_expected, 1E-5,
|
|
MatrixCompareType::absolute));
|
|
}
|
|
}
|
|
}
|
|
|
|
GTEST_TEST(TestSos2, TestClosestPointOnLineSegments) {
|
|
// We will define line segments A₀A₁, ..., A₅A₆ in 2D, where points Aᵢ are
|
|
// defined as A₀ = (0, 0), A₁ = (1, 1), A₂ = (2, 0), A₃ = (4, 2), A₅ = (6, 0),
|
|
// A₅ = (7, 1), A₆ = (8, 0). We compute the closest point P = (x, y) on the
|
|
// line segments to a given point Q.
|
|
MathematicalProgram prog;
|
|
auto x = prog.NewContinuousVariables<1>()(0);
|
|
auto y = prog.NewContinuousVariables<1>()(0);
|
|
Eigen::Matrix<double, 2, 7> A;
|
|
// clang-format off
|
|
A << 0, 1, 2, 4, 6, 7, 8,
|
|
0, 1, 0, 2, 0, 1, 0;
|
|
// clang-format on
|
|
auto lambda = prog.NewContinuousVariables<7>();
|
|
auto z = prog.NewBinaryVariables<6>();
|
|
AddSos2Constraint(&prog, lambda.cast<symbolic::Expression>(),
|
|
z.cast<symbolic::Expression>());
|
|
const Vector2<symbolic::Expression> line_segment = A * lambda;
|
|
prog.AddLinearConstraint(line_segment(0) == x);
|
|
prog.AddLinearConstraint(line_segment(1) == y);
|
|
|
|
// Add a dummy cost function, which we will change in the for loop below.
|
|
Binding<QuadraticCost> cost =
|
|
prog.AddQuadraticCost(Eigen::Matrix2d::Zero(), Eigen::Vector2d::Zero(), 0,
|
|
VectorDecisionVariable<2>(x, y));
|
|
// We will test with different points Qs, each Q corresponds to a nearest
|
|
// point P on the line segments.
|
|
std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d>> Q_and_P;
|
|
Q_and_P.push_back(
|
|
std::make_pair(Eigen::Vector2d(1, 1), Eigen::Vector2d(1, 1)));
|
|
Q_and_P.push_back(
|
|
std::make_pair(Eigen::Vector2d(1.9, 1), Eigen::Vector2d(1.45, 0.55)));
|
|
Q_and_P.push_back(
|
|
std::make_pair(Eigen::Vector2d(3, 1), Eigen::Vector2d(3, 1)));
|
|
Q_and_P.push_back(
|
|
std::make_pair(Eigen::Vector2d(5, 1.2), Eigen::Vector2d(4.9, 1.1)));
|
|
Q_and_P.push_back(
|
|
std::make_pair(Eigen::Vector2d(7.5, 1.2), Eigen::Vector2d(7.15, 0.85)));
|
|
for (const auto& QP_pair : Q_and_P) {
|
|
const Eigen::Vector2d& Q = QP_pair.first;
|
|
// The cost is |P-Q|²
|
|
cost.evaluator()->UpdateCoefficients(2 * Eigen::Matrix2d::Identity(),
|
|
-2 * Q, Q.squaredNorm());
|
|
|
|
// Any mixed integer convex solver can solve this problem, here we choose
|
|
// gurobi.
|
|
GurobiSolver gurobi_solver;
|
|
if (gurobi_solver.available()) {
|
|
auto result = gurobi_solver.Solve(prog, {}, {});
|
|
EXPECT_TRUE(result.is_success());
|
|
const Eigen::Vector2d P(
|
|
result.GetSolution(VectorDecisionVariable<2>(x, y)));
|
|
const Eigen::Vector2d P_expected = QP_pair.second;
|
|
EXPECT_TRUE(CompareMatrices(P, P_expected, 1E-6));
|
|
EXPECT_NEAR(result.get_optimal_cost(), (Q - P_expected).squaredNorm(),
|
|
1E-12);
|
|
}
|
|
}
|
|
}
|
|
|
|
GTEST_TEST(TestLogarithmicSos2, Test4Lambda) {
|
|
LogarithmicSos2Test(4, true);
|
|
}
|
|
|
|
GTEST_TEST(TestLogarithmicSos2, Test5Lambda) {
|
|
LogarithmicSos2Test(5, true);
|
|
}
|
|
|
|
GTEST_TEST(TestLogarithmicSos2, Test6Lambda) {
|
|
LogarithmicSos2Test(6, true);
|
|
}
|
|
|
|
GTEST_TEST(TestLogarithmicSos2, Test7Lambda) {
|
|
LogarithmicSos2Test(7, true);
|
|
}
|
|
|
|
GTEST_TEST(TestLogarithmicSos2, Test8Lambda) {
|
|
LogarithmicSos2Test(8, true);
|
|
}
|
|
|
|
GTEST_TEST(TestSos2, Test4Lambda) {
|
|
LogarithmicSos2Test(4, false);
|
|
}
|
|
|
|
GTEST_TEST(TestSos2, Test5Lambda) {
|
|
LogarithmicSos2Test(5, false);
|
|
}
|
|
|
|
GTEST_TEST(TestSos2, Test6Lambda) {
|
|
LogarithmicSos2Test(6, false);
|
|
}
|
|
|
|
GTEST_TEST(TestSos2, Test7Lambda) {
|
|
LogarithmicSos2Test(7, false);
|
|
}
|
|
|
|
GTEST_TEST(TestSos2, Test8Lambda) {
|
|
LogarithmicSos2Test(8, false);
|
|
}
|
|
|
|
void LogarithmicSos1Test(int num_lambda,
|
|
const Eigen::Ref<const Eigen::MatrixXi>& codes) {
|
|
// Check if we impose the constraint
|
|
// λ is in sos1
|
|
// and assign values to the binary variables,
|
|
// whether the corresponding λ(i) is 1.
|
|
MathematicalProgram prog;
|
|
auto lambda = prog.NewContinuousVariables(num_lambda);
|
|
int num_digits = CeilLog2(num_lambda);
|
|
auto y = prog.NewBinaryVariables(num_digits);
|
|
AddLogarithmicSos1Constraint(&prog, lambda.cast<symbolic::Expression>(), y,
|
|
codes);
|
|
auto binary_assignment = prog.AddBoundingBoxConstraint(0, 0, y);
|
|
for (int i = 0; i < num_lambda; ++i) {
|
|
Eigen::VectorXd code = codes.row(i).cast<double>().transpose();
|
|
binary_assignment.evaluator()->UpdateLowerBound(code);
|
|
binary_assignment.evaluator()->UpdateUpperBound(code);
|
|
GurobiSolver gurobi_solver;
|
|
if (gurobi_solver.available()) {
|
|
auto result = gurobi_solver.Solve(prog, {}, {});
|
|
EXPECT_TRUE(result.is_success());
|
|
Eigen::VectorXd lambda_expected(num_lambda);
|
|
lambda_expected.setZero();
|
|
lambda_expected(i) = 1;
|
|
EXPECT_TRUE(CompareMatrices(result.GetSolution(lambda), lambda_expected,
|
|
1E-6, MatrixCompareType::absolute));
|
|
}
|
|
}
|
|
}
|
|
|
|
GTEST_TEST(TestLogarithmicSos1, Test2Lambda) {
|
|
Eigen::Matrix<int, 2, 1> codes;
|
|
codes << 0, 1;
|
|
LogarithmicSos1Test(2, codes);
|
|
// Test a different codes
|
|
codes << 1, 0;
|
|
LogarithmicSos1Test(2, codes);
|
|
}
|
|
|
|
GTEST_TEST(TestLogarithmicSos1, Test3Lambda) {
|
|
Eigen::Matrix<int, 3, 2> codes;
|
|
// clang-format off
|
|
codes << 0, 0,
|
|
0, 1,
|
|
1, 0;
|
|
// clang-format on
|
|
LogarithmicSos1Test(3, codes);
|
|
// Test a different codes
|
|
// clang-format off
|
|
codes << 0, 0,
|
|
0, 1,
|
|
1, 1;
|
|
// clang-format on
|
|
LogarithmicSos1Test(3, codes);
|
|
}
|
|
|
|
GTEST_TEST(TestLogarithmicSos1, Test4Lambda) {
|
|
Eigen::Matrix<int, 4, 2> codes;
|
|
// clang-format off
|
|
codes << 0, 0,
|
|
0, 1,
|
|
1, 0,
|
|
1, 1;
|
|
// clang-format on
|
|
LogarithmicSos1Test(4, codes);
|
|
// Test a different codes
|
|
// clang-format off
|
|
codes << 0, 0,
|
|
0, 1,
|
|
1, 1,
|
|
1, 0;
|
|
// clang-format on
|
|
LogarithmicSos1Test(4, codes);
|
|
}
|
|
|
|
GTEST_TEST(TestLogarithmicSos1, Test5Lambda) {
|
|
auto codes = math::CalculateReflectedGrayCodes<3>();
|
|
LogarithmicSos1Test(5, codes.topRows<5>());
|
|
}
|
|
|
|
GTEST_TEST(TestLogarithmicSos1, Test) {
|
|
MathematicalProgram prog;
|
|
VectorX<symbolic::Variable> y, lambda;
|
|
std::tie(lambda, y) = AddLogarithmicSos1Constraint(&prog, 3);
|
|
EXPECT_EQ(lambda.rows(), 3);
|
|
EXPECT_EQ(y.rows(), 2);
|
|
|
|
auto check = [&prog, &y, &lambda](const Eigen::VectorXd& lambda_val,
|
|
const Eigen::VectorXd& y_val,
|
|
bool satisfied_expected) {
|
|
Eigen::VectorXd x_val = Eigen::VectorXd::Zero(prog.num_vars());
|
|
prog.SetDecisionVariableValueInVector(y, y_val, &x_val);
|
|
prog.SetDecisionVariableValueInVector(lambda, lambda_val, &x_val);
|
|
bool satisfied = true;
|
|
for (const auto& binding : prog.GetAllConstraints()) {
|
|
satisfied =
|
|
satisfied && binding.evaluator()->CheckSatisfied(
|
|
prog.GetBindingVariableValues(binding, x_val));
|
|
}
|
|
EXPECT_EQ(satisfied, satisfied_expected);
|
|
};
|
|
check(Eigen::Vector3d(0, 0, 1), Eigen::Vector2d(1, 1), true);
|
|
check(Eigen::Vector3d(1, 0, 0), Eigen::Vector2d(1, 0), false);
|
|
check(Eigen::Vector3d(0, 1, 0), Eigen::Vector2d(0, 1), true);
|
|
check(Eigen::Vector3d(0, 0.5, 0.5), Eigen::Vector2d(1, 1), false);
|
|
check(Eigen::Vector3d(0, 0.1, 1), Eigen::Vector2d(1, 0), false);
|
|
}
|
|
|
|
GTEST_TEST(TestBilinearProductMcCormickEnvelopeSos2, AddConstraint) {
|
|
// Test if the return argument from
|
|
// AddBilinearProductMcCormickEnvelopeSos2 has the right type.
|
|
MathematicalProgram prog;
|
|
auto x = prog.NewContinuousVariables<1>()(0);
|
|
auto y = prog.NewContinuousVariables<1>()(0);
|
|
auto w = prog.NewContinuousVariables<1>()(0);
|
|
|
|
const Eigen::Vector3d phi_x_static(0, 1, 2);
|
|
Eigen::VectorXd phi_x_dynamic = Eigen::VectorXd::LinSpaced(3, 0, 2);
|
|
const Eigen::Vector4d phi_y_static = Eigen::Vector4d::LinSpaced(0, 3);
|
|
Eigen::VectorXd phi_y_dynamic = Eigen::VectorXd::LinSpaced(4, 0, 3);
|
|
Vector1<symbolic::Expression> Bx = prog.NewBinaryVariables<1>();
|
|
Vector2<symbolic::Expression> By = prog.NewBinaryVariables<2>();
|
|
auto lambda1 = AddBilinearProductMcCormickEnvelopeSos2(
|
|
&prog, x, y, w, phi_x_static, phi_y_static, Bx, By,
|
|
IntervalBinning::kLogarithmic);
|
|
static_assert(std::is_same_v<decltype(lambda1), MatrixDecisionVariable<3, 4>>,
|
|
"lambda should be a static matrix");
|
|
|
|
auto lambda2 = AddBilinearProductMcCormickEnvelopeSos2(
|
|
&prog, x, y, w, phi_x_dynamic, phi_y_static, Bx, By,
|
|
IntervalBinning::kLogarithmic);
|
|
static_assert(std::is_same_v<decltype(lambda2),
|
|
MatrixDecisionVariable<Eigen::Dynamic, 4>>,
|
|
"lambda's type is incorrect");
|
|
|
|
auto lambda3 = AddBilinearProductMcCormickEnvelopeSos2(
|
|
&prog, x, y, w, phi_x_static, phi_y_dynamic, Bx, By,
|
|
IntervalBinning::kLogarithmic);
|
|
static_assert(std::is_same_v<decltype(lambda3),
|
|
MatrixDecisionVariable<3, Eigen::Dynamic>>,
|
|
"lambda's type is incorrect");
|
|
|
|
auto lambda4 = AddBilinearProductMcCormickEnvelopeSos2(
|
|
&prog, x, y, w, phi_x_dynamic, phi_y_dynamic, Bx, By,
|
|
IntervalBinning::kLogarithmic);
|
|
static_assert(
|
|
std::is_same_v<decltype(lambda4),
|
|
MatrixDecisionVariable<Eigen::Dynamic, Eigen::Dynamic>>,
|
|
"lambda's type is incorrect");
|
|
}
|
|
|
|
class BilinearProductMcCormickEnvelopeTest {
|
|
public:
|
|
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(BilinearProductMcCormickEnvelopeTest)
|
|
|
|
BilinearProductMcCormickEnvelopeTest(int num_interval_x, int num_interval_y)
|
|
: prog_{},
|
|
prog_result_{},
|
|
num_interval_x_{num_interval_x},
|
|
num_interval_y_{num_interval_y},
|
|
w_{prog_.NewContinuousVariables<1>()(0)},
|
|
x_{prog_.NewContinuousVariables<1>()(0)},
|
|
y_{prog_.NewContinuousVariables<1>()(0)},
|
|
phi_x_{Eigen::VectorXd::LinSpaced(num_interval_x_ + 1, 0, 1)},
|
|
phi_y_{Eigen::VectorXd::LinSpaced(num_interval_y_ + 1, 0, 1)} {}
|
|
|
|
virtual ~BilinearProductMcCormickEnvelopeTest() = default;
|
|
|
|
virtual Eigen::VectorXd SetBinaryValue(int active_interval,
|
|
int num_interval) const = 0;
|
|
|
|
Eigen::VectorXd SetBinaryValueLinearBinning(int active_interval,
|
|
int num_interval) const {
|
|
Eigen::VectorXd b = Eigen::VectorXd::Zero(num_interval);
|
|
b(active_interval) = 1;
|
|
return b;
|
|
}
|
|
|
|
void TestFeasiblePoint() {
|
|
auto x_constraint = prog_.AddBoundingBoxConstraint(0, 0, x_);
|
|
auto y_constraint = prog_.AddBoundingBoxConstraint(0, 0, y_);
|
|
auto w_constraint = prog_.AddBoundingBoxConstraint(0, 0, w_);
|
|
|
|
auto CheckFeasibility = [&x_constraint, &y_constraint, &w_constraint](
|
|
MathematicalProgram* prog, double x_val,
|
|
double y_val, double w_val, bool is_feasible) {
|
|
auto UpdateBound = [](Binding<BoundingBoxConstraint>* constraint,
|
|
double val) {
|
|
constraint->evaluator()->UpdateLowerBound(Vector1d(val));
|
|
constraint->evaluator()->UpdateUpperBound(Vector1d(val));
|
|
};
|
|
UpdateBound(&x_constraint, x_val);
|
|
UpdateBound(&y_constraint, y_val);
|
|
UpdateBound(&w_constraint, w_val);
|
|
|
|
prog->SetSolverOption(GurobiSolver::id(), "DualReductions", 0);
|
|
|
|
GurobiSolver solver;
|
|
if (solver.available()) {
|
|
MathematicalProgramResult result;
|
|
solver.Solve(*prog, {}, {}, &result);
|
|
EXPECT_EQ(result.get_solution_result(),
|
|
is_feasible ? SolutionResult::kSolutionFound
|
|
: SolutionResult::kInfeasibleConstraints);
|
|
}
|
|
};
|
|
|
|
// Feasible points
|
|
CheckFeasibility(&prog_, 0, 0, 0, true);
|
|
CheckFeasibility(&prog_, 0, 1, 0, true);
|
|
CheckFeasibility(&prog_, 1, 0, 0, true);
|
|
CheckFeasibility(&prog_, 1, 1, 1, true);
|
|
CheckFeasibility(&prog_, 0.5, 1, 0.5, true);
|
|
CheckFeasibility(&prog_, 1, 0.5, 0.5, true);
|
|
CheckFeasibility(&prog_, 0.5, 0.5, 0.25, true);
|
|
|
|
if (num_interval_x_ == 2 && num_interval_y_ == 2) {
|
|
CheckFeasibility(&prog_, 0.5, 0.5, 0.26, false);
|
|
CheckFeasibility(&prog_, 0.25, 0.25, 0.1, true);
|
|
CheckFeasibility(&prog_, 0.25, 0.25, 0.126, false);
|
|
CheckFeasibility(&prog_, 0.5, 0.6, 0.301, false);
|
|
}
|
|
if (num_interval_x_ == 3 && num_interval_y_ == 3) {
|
|
CheckFeasibility(&prog_, 1.0 / 3, 1.0 / 3, 0.1, false);
|
|
CheckFeasibility(&prog_, 0.5, 0.5, 0.26, true);
|
|
CheckFeasibility(&prog_, 1.0 / 3, 2.0 / 3, 2.0 / 9, true);
|
|
CheckFeasibility(&prog_, 0.5, 0.5, 5.0 / 18, true);
|
|
CheckFeasibility(&prog_, 0.5, 0.5, 5.0 / 18 + 0.001, false);
|
|
}
|
|
}
|
|
|
|
void TestLinearObjective() {
|
|
// We will assign the binary variables Bx_ and By_ to determine which
|
|
// interval is active. If we use logarithmic binning, then Bx_ and By_ take
|
|
// values in the gray code, representing integer i and j, such that x is
|
|
// constrained in [φx(i), φx(i+1)], y is constrained in [φy(j), φy(j+1)].
|
|
auto Bx_constraint =
|
|
prog_.AddBoundingBoxConstraint(Eigen::VectorXd::Zero(Bx_.rows()),
|
|
Eigen::VectorXd::Zero(Bx_.rows()), Bx_);
|
|
auto By_constraint =
|
|
prog_.AddBoundingBoxConstraint(Eigen::VectorXd::Zero(By_.rows()),
|
|
Eigen::VectorXd::Zero(By_.rows()), By_);
|
|
VectorDecisionVariable<3> xyw{x_, y_, w_};
|
|
auto cost = prog_.AddLinearCost(Eigen::Vector3d::Zero(), xyw);
|
|
Eigen::Matrix<double, 3, 8> a;
|
|
// clang-format off
|
|
a << 1, 1, 1, 1, -1, -1, -1, -1,
|
|
1, 1, -1, -1, 1, 1, -1, -1,
|
|
1, -1, 1, -1, 1, -1, 1, -1;
|
|
// clang-format on
|
|
for (int i = 0; i < num_interval_x_; ++i) {
|
|
const auto Bx_val = SetBinaryValue(i, num_interval_x_);
|
|
|
|
Bx_constraint.evaluator()->UpdateLowerBound(Bx_val);
|
|
Bx_constraint.evaluator()->UpdateUpperBound(Bx_val);
|
|
for (int j = 0; j < num_interval_y_; ++j) {
|
|
const auto By_val = SetBinaryValue(j, num_interval_y_);
|
|
By_constraint.evaluator()->UpdateLowerBound(By_val);
|
|
By_constraint.evaluator()->UpdateUpperBound(By_val);
|
|
|
|
// vertices.col(l) is the l'th vertex of the tetrahedron.
|
|
Eigen::Matrix<double, 3, 4> vertices;
|
|
vertices.row(0) << phi_x_(i), phi_x_(i), phi_x_(i + 1), phi_x_(i + 1);
|
|
vertices.row(1) << phi_y_(j), phi_y_(j + 1), phi_y_(j), phi_y_(j + 1);
|
|
vertices.row(2) = vertices.row(0).cwiseProduct(vertices.row(1));
|
|
for (int k = 0; k < a.cols(); ++k) {
|
|
cost.evaluator()->UpdateCoefficients(a.col(k));
|
|
GurobiSolver gurobi_solver;
|
|
if (gurobi_solver.available()) {
|
|
gurobi_solver.Solve(prog_, {}, {}, &prog_result_);
|
|
EXPECT_TRUE(prog_result_.is_success());
|
|
Eigen::Matrix<double, 1, 4> cost_at_vertices =
|
|
a.col(k).transpose() * vertices;
|
|
EXPECT_NEAR(prog_result_.get_optimal_cost(),
|
|
cost_at_vertices.minCoeff(), 1E-4);
|
|
|
|
TestLinearObjectiveCheck(i, j, k);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
virtual void TestLinearObjectiveCheck(int i, int j, int k) const {}
|
|
|
|
protected:
|
|
MathematicalProgram prog_;
|
|
MathematicalProgramResult prog_result_;
|
|
const int num_interval_x_;
|
|
const int num_interval_y_;
|
|
const symbolic::Variable w_;
|
|
const symbolic::Variable x_;
|
|
const symbolic::Variable y_;
|
|
const Eigen::VectorXd phi_x_;
|
|
const Eigen::VectorXd phi_y_;
|
|
VectorXDecisionVariable Bx_;
|
|
VectorXDecisionVariable By_;
|
|
};
|
|
|
|
class BilinearProductMcCormickEnvelopeSos2Test
|
|
: public ::testing::TestWithParam<std::tuple<int, int, IntervalBinning>>,
|
|
public BilinearProductMcCormickEnvelopeTest {
|
|
public:
|
|
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(BilinearProductMcCormickEnvelopeSos2Test)
|
|
|
|
BilinearProductMcCormickEnvelopeSos2Test()
|
|
: BilinearProductMcCormickEnvelopeTest(std::get<0>(GetParam()),
|
|
std::get<1>(GetParam())),
|
|
binning_{std::get<2>(GetParam())},
|
|
Bx_size_{binning_ == IntervalBinning::kLogarithmic
|
|
? CeilLog2(num_interval_x_)
|
|
: num_interval_x_},
|
|
By_size_{binning_ == IntervalBinning::kLogarithmic
|
|
? CeilLog2(num_interval_y_)
|
|
: num_interval_y_} {
|
|
Bx_ = prog_.NewBinaryVariables(Bx_size_);
|
|
By_ = prog_.NewBinaryVariables(By_size_);
|
|
lambda_ = AddBilinearProductMcCormickEnvelopeSos2(
|
|
&prog_, x_, y_, w_, phi_x_, phi_y_, Bx_.cast<symbolic::Expression>(),
|
|
By_.cast<symbolic::Expression>(), binning_);
|
|
}
|
|
|
|
Eigen::VectorXd SetBinaryValueLogarithmicBinning(int active_interval,
|
|
int num_interval) const {
|
|
const Eigen::MatrixXi gray_codes =
|
|
math::CalculateReflectedGrayCodes(solvers::CeilLog2(num_interval));
|
|
return gray_codes.row(active_interval).cast<double>().transpose();
|
|
}
|
|
|
|
Eigen::VectorXd SetBinaryValue(int active_interval,
|
|
int num_interval) const override {
|
|
switch (binning_) {
|
|
case IntervalBinning::kLinear: {
|
|
return SetBinaryValueLinearBinning(active_interval, num_interval);
|
|
}
|
|
case IntervalBinning::kLogarithmic: {
|
|
return SetBinaryValueLogarithmicBinning(active_interval, num_interval);
|
|
}
|
|
default: {
|
|
throw std::runtime_error(
|
|
"This default case should not be reached. We add the default case "
|
|
"due to a gcc-5 pitfall.");
|
|
}
|
|
}
|
|
}
|
|
|
|
void TestLinearObjectiveCheck(int i, int j, int k) const override {
|
|
// Check that λ has the correct value, except λ(i, j), λ(i, j+1),
|
|
// λ(i+1, j) and λ(i+1, j+1), all other entries in λ are zero.
|
|
double w{0};
|
|
double x{0};
|
|
double y{0};
|
|
for (int m = 0; m <= num_interval_x_; ++m) {
|
|
for (int n = 0; n <= num_interval_y_; ++n) {
|
|
if (!((m == i && n == j) || (m == i && n == (j + 1)) ||
|
|
(m == (i + 1) && n == j) || (m == (i + 1) && n == (j + 1)))) {
|
|
EXPECT_NEAR(prog_result_.GetSolution(lambda_(m, n)), 0, 1E-5);
|
|
} else {
|
|
double lambda_mn{prog_result_.GetSolution(lambda_(m, n))};
|
|
x += lambda_mn * phi_x_(m);
|
|
y += lambda_mn * phi_y_(n);
|
|
w += lambda_mn * phi_x_(m) * phi_y_(n);
|
|
}
|
|
}
|
|
}
|
|
EXPECT_NEAR(prog_result_.GetSolution(x_), x, 1E-4);
|
|
EXPECT_NEAR(prog_result_.GetSolution(y_), y, 1E-4);
|
|
EXPECT_NEAR(prog_result_.GetSolution(w_), w, 1E-4);
|
|
}
|
|
|
|
protected:
|
|
const IntervalBinning binning_;
|
|
const int Bx_size_;
|
|
const int By_size_;
|
|
MatrixXDecisionVariable lambda_;
|
|
};
|
|
|
|
TEST_P(BilinearProductMcCormickEnvelopeSos2Test, LinearObjectiveTest) {
|
|
TestLinearObjective();
|
|
}
|
|
|
|
TEST_P(BilinearProductMcCormickEnvelopeSos2Test, FeasiblePointTest) {
|
|
TestFeasiblePoint();
|
|
}
|
|
|
|
INSTANTIATE_TEST_SUITE_P(
|
|
TestMixedIntegerUtil, BilinearProductMcCormickEnvelopeSos2Test,
|
|
::testing::Combine(::testing::ValuesIn(std::vector<int>{2, 3}),
|
|
::testing::ValuesIn(std::vector<int>{2, 3}),
|
|
::testing::ValuesIn(std::vector<IntervalBinning>{
|
|
IntervalBinning::kLogarithmic,
|
|
IntervalBinning::kLinear})));
|
|
|
|
class BilinearProductMcCormickEnvelopeMultipleChoiceTest
|
|
: public ::testing::TestWithParam<std::tuple<int, int>>,
|
|
public BilinearProductMcCormickEnvelopeTest {
|
|
public:
|
|
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(
|
|
BilinearProductMcCormickEnvelopeMultipleChoiceTest)
|
|
|
|
BilinearProductMcCormickEnvelopeMultipleChoiceTest()
|
|
: BilinearProductMcCormickEnvelopeTest(std::get<0>(GetParam()),
|
|
std::get<1>(GetParam())) {
|
|
Bx_ = prog_.NewBinaryVariables(num_interval_x_);
|
|
By_ = prog_.NewBinaryVariables(num_interval_y_);
|
|
AddBilinearProductMcCormickEnvelopeMultipleChoice(
|
|
&prog_, x_, y_, w_, phi_x_, phi_y_, Bx_.cast<symbolic::Expression>(),
|
|
By_.cast<symbolic::Expression>());
|
|
}
|
|
|
|
Eigen::VectorXd SetBinaryValue(int active_interval,
|
|
int num_interval) const override {
|
|
return SetBinaryValueLinearBinning(active_interval, num_interval);
|
|
}
|
|
};
|
|
|
|
TEST_P(BilinearProductMcCormickEnvelopeMultipleChoiceTest,
|
|
LinearObjectiveTest) {
|
|
TestLinearObjective();
|
|
}
|
|
|
|
TEST_P(BilinearProductMcCormickEnvelopeMultipleChoiceTest, FeasiblePointTest) {
|
|
TestFeasiblePoint();
|
|
}
|
|
|
|
INSTANTIATE_TEST_SUITE_P(
|
|
TestMixedIntegerUtil, BilinearProductMcCormickEnvelopeMultipleChoiceTest,
|
|
::testing::Combine(::testing::ValuesIn(std::vector<int>{2, 3}),
|
|
::testing::ValuesIn(std::vector<int>{2, 3})));
|
|
} // namespace
|
|
} // namespace solvers
|
|
} // namespace drake
|