-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
linear_system_solver_test.cc
121 lines (101 loc) · 3.84 KB
/
linear_system_solver_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
#include "drake/solvers/linear_system_solver.h"
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/common/test_utilities/expect_throws_message.h"
#include "drake/solvers/solve.h"
#include "drake/solvers/test/mathematical_program_test_util.h"
#include "drake/solvers/test/optimization_examples.h"
using ::testing::HasSubstr;
namespace drake {
namespace solvers {
namespace test {
namespace {
void TestLinearSystemExample(LinearSystemExample1* example) {
const MathematicalProgram& prog = *example->prog();
EXPECT_TRUE(LinearSystemSolver::ProgramAttributesSatisfied(prog));
EXPECT_EQ(LinearSystemSolver::UnsatisfiedProgramAttributes(prog), "");
const MathematicalProgramResult result = Solve(prog);
EXPECT_TRUE(result.is_success());
example->CheckSolution(result);
}
} // namespace
GTEST_TEST(testLinearSystemSolver, trivialExample) {
LinearSystemExample1 example1{};
TestLinearSystemExample(&example1);
LinearSystemExample2 example2{};
TestLinearSystemExample(&example2);
LinearSystemExample3 example3{};
TestLinearSystemExample(&example3);
}
GTEST_TEST(testLinearSystemSolver, EmptyProblem) {
MathematicalProgram prog;
EXPECT_FALSE(LinearSystemSolver::ProgramAttributesSatisfied(prog));
EXPECT_THAT(LinearSystemSolver::UnsatisfiedProgramAttributes(prog),
HasSubstr("LinearEqualityConstraint is required"));
LinearSystemSolver solver;
DRAKE_EXPECT_THROWS_MESSAGE(
solver.Solve(prog),
".*LinearEqualityConstraint is required.*");
}
GTEST_TEST(testLinearSystemSolver, QuadraticProblem) {
MathematicalProgram prog;
auto x = prog.NewContinuousVariables<1>();
prog.AddQuadraticCost(x(0) * x(0));
EXPECT_FALSE(LinearSystemSolver::ProgramAttributesSatisfied(prog));
EXPECT_THAT(LinearSystemSolver::UnsatisfiedProgramAttributes(prog),
HasSubstr("QuadraticCost was declared"));
LinearSystemSolver solver;
DRAKE_EXPECT_THROWS_MESSAGE(
solver.Solve(prog),
".*QuadraticCost was declared.*");
}
/**
* Simple linear system without a solution
* 3 * x = 1
* 2 * x + y = 2
* x - y = 0
*/
GTEST_TEST(testLinearSystemSolver, InfeasibleProblem) {
MathematicalProgram prog;
auto x = prog.NewContinuousVariables<2>();
prog.AddLinearConstraint(3 * x(0) == 1 && 2 * x(0) + x(1) == 2 &&
x(0) - x(1) == 0);
const MathematicalProgramResult result = Solve(prog);
EXPECT_FALSE(result.is_success());
// The solution should minimize the error ||b - A * x||₂
// x_expected is computed as (Aᵀ*A)⁻¹*(Aᵀ*b)
Eigen::Vector2d x_expected(12.0 / 27, 21.0 / 27);
EXPECT_TRUE(CompareMatrices(result.GetSolution(x), x_expected, 1E-12,
MatrixCompareType::absolute));
EXPECT_EQ(result.get_optimal_cost(),
MathematicalProgram::kGlobalInfeasibleCost);
}
/**
* Under-determined linear system
* 3 * x + y = 1
* x + z = 2
*/
GTEST_TEST(testLinearSystemSolver, UnderDeterminedProblem) {
MathematicalProgram prog;
auto x = prog.NewContinuousVariables<3>();
prog.AddLinearConstraint(3 * x(0) + x(1) == 1 && x(0) + x(2) == 2);
const MathematicalProgramResult result = Solve(prog);
EXPECT_TRUE(result.is_success());
// The solution should minimize the norm ||x||₂
// x_expected is computed as the solution to
// [2*I -Aᵀ] * [x] = [0]
// [ A 0 ] [λ] [b]
Eigen::Vector3d x_expected(5.0 / 11, -4.0 / 11, 17.0 / 11);
EXPECT_TRUE(CompareMatrices(result.GetSolution(x), x_expected, 1E-12,
MatrixCompareType::absolute));
}
GTEST_TEST(testLinearSystemSolver, linearMatrixEqualityExample) {
LinearMatrixEqualityExample example{};
const auto result = Solve(*(example.prog()));
EXPECT_EQ(result.get_solver_id(), LinearSystemSolver::id());
example.CheckSolution(result);
}
} // namespace test
} // namespace solvers
} // namespace drake