/
second_order_cone_program_examples.h
336 lines (291 loc) · 11.2 KB
/
second_order_cone_program_examples.h
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
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
#pragma once
#include <memory>
#include <vector>
#include <gtest/gtest.h>
#include "drake/common/test_utilities/eigen_matrix_compare.h"
#include "drake/solvers/mathematical_program.h"
#include "drake/solvers/solver_interface.h"
namespace drake {
namespace solvers {
namespace test {
enum class EllipsoidsSeparationProblem {
kProblem0,
kProblem1,
kProblem2,
kProblem3
};
std::vector<EllipsoidsSeparationProblem> GetEllipsoidsSeparationProblems();
/// This test is taken from the course notes of EE127A from University of
/// California, Berkeley
/// The goal is to find a hyperplane, that separates two ellipsoids
/// E₁ = x₁ + R₁ * u₁, |u₁| ≤ 1
/// E₂ = x₂ + R₂ * u₂, |u₂| ≤ 1
/// A hyperplane aᵀ * x = b separates these two ellipsoids, if and only if for
/// SOCP p* = min t₁ + t₂
/// s.t t₁ ≥ |R₁ᵀ * a|
/// t₂ ≥ |R₂ᵀ * a|
/// aᵀ(x₂-x₁) = 1
/// the optimal solution p* is no larger than 1. In that case, an appropriate
/// value of b is b = 0.5(b₁ + b₂), where
/// b₁ = aᵀx₁ + |R₁ᵀ * a|
/// b₂ = aᵀx₂ - |R₂ᵀ * a|
/// @param x1 the center of ellipsoid 1
/// @param x2 the center of ellipsoid 2
/// @param R1 the shape of ellipsoid 1
/// @param R2 the shape of ellipsoid 2
class TestEllipsoidsSeparation
: public ::testing::TestWithParam<EllipsoidsSeparationProblem> {
public:
TestEllipsoidsSeparation();
void SolveAndCheckSolution(const SolverInterface& solver,
double tol = 1E-8);
private:
Eigen::VectorXd x1_;
Eigen::VectorXd x2_;
Eigen::MatrixXd R1_;
Eigen::MatrixXd R2_;
MathematicalProgram prog_;
VectorDecisionVariable<2> t_;
VectorXDecisionVariable a_;
};
enum class QPasSOCPProblem { kProblem0, kProblem1 };
std::vector<QPasSOCPProblem> GetQPasSOCPProblems();
/// This example is taken from the course notes of EE127A from University of
/// California, Berkeley
/// For a quadratic program
/// 0.5 * x' * Q * x + c' * x
/// s.t b_lb <= A * x <= b_ub
/// It can be casted as an SOCP, as follows
/// By introducing a new variable w = Q^{1/2}*x and y, z
/// The equivalent SOCP is
/// min c'x + y
/// s.t 2 * y >= w' * w
/// w = Q^{1/2} * x
/// b_lb <= A * x <= b_ub
/// @param Q A positive definite matrix
/// @param c A column vector
/// @param A A matrix
/// @param b_lb A column vector
/// @param b_ub A column vector
class TestQPasSOCP : public ::testing::TestWithParam<QPasSOCPProblem> {
public:
TestQPasSOCP();
void SolveAndCheckSolution(const SolverInterface& solver,
double tol = 1E-6);
private:
Eigen::MatrixXd Q_;
Eigen::VectorXd c_;
Eigen::MatrixXd A_;
Eigen::VectorXd b_lb_;
Eigen::VectorXd b_ub_;
MathematicalProgram prog_socp_;
MathematicalProgram prog_qp_;
VectorXDecisionVariable x_socp_;
symbolic::Variable y_;
VectorXDecisionVariable x_qp_;
};
/// This example is taken from the paper
/// Applications of second-order cone programming
/// By M.S.Lobo, L.Vandenberghe, S.Boyd and H.Lebret,
/// Section 3.6
/// http://www.seas.ucla.edu/~vandenbe/publications/socp.pdf
/// The problem tries to find the equilibrium state of a mechanical
/// system, which consists of n nodes at position (x₁,y₁), (x₂,y₂), ...,
/// (xₙ,yₙ) in ℝ².
/// The nodes are connected by springs with given coefficient.
/// The spring generate force when it is stretched,
/// but not when it is compressed.
/// Namely, the spring force is
/// (spring_length - spring_rest_length) * spring_stiffness,
/// if spring_length ≥ spring_rest_length;
/// otherwise the spring force is zero.
/// weightᵢ is the mass * gravity_acceleration
/// of the i'th link.
/// The equilibrium point is obtained when the total energy is minimized
/// namely min ∑ᵢ weightᵢ * yᵢ + stiffness/2 * tᵢ²
/// s.t sqrt((xᵢ - xᵢ₊₁)² + (yᵢ - yᵢ₊₁)²) - spring_rest_length ≤ tᵢ
/// 0 ≤ tᵢ
/// (x₁,y₁) = end_pos1
/// (xₙ,yₙ) = end_pos2
/// By introducing a slack variable z ≥ t₁² + ... + tₙ₋₁², the problem
/// becomes
/// an SOCP, with both Lorentz cone and rotated Lorentz cone constraints
enum class FindSpringEquilibriumProblem { kProblem0 };
std::vector<FindSpringEquilibriumProblem> GetFindSpringEquilibriumProblems();
class TestFindSpringEquilibrium
: public ::testing::TestWithParam<FindSpringEquilibriumProblem> {
public:
TestFindSpringEquilibrium();
void SolveAndCheckSolution(const SolverInterface& solver,
double tol = 2E-3);
private:
Eigen::VectorXd weight_;
double spring_rest_length_;
double spring_stiffness_;
Eigen::Vector2d end_pos1_;
Eigen::Vector2d end_pos2_;
MathematicalProgram prog_;
VectorXDecisionVariable x_;
VectorXDecisionVariable y_;
VectorXDecisionVariable t_;
symbolic::Variable z_;
};
/**
* Solve the following trivial problem
* max (2x+3)*(3x+2)
* s.t 2x+3 >= 0
* 3x+2 >= 0
* x<= 10
* We formulate this problem as a convex second order cone constraint, by
* regarding the cost as the square of geometric mean between 2x+3 and 3x+2.
* The optimal is x* = 10.
*/
class MaximizeGeometricMeanTrivialProblem1 {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(MaximizeGeometricMeanTrivialProblem1)
MaximizeGeometricMeanTrivialProblem1();
const MathematicalProgram& prog() const { return *prog_; }
void CheckSolution(const MathematicalProgramResult& result, double tol);
private:
std::unique_ptr<MathematicalProgram> prog_;
symbolic::Variable x_;
};
/**
* Solve the following trivial problem
* max (2x+3)*(3x+2)*(4x+5)
* s.t 2x+3 >= 0
* 3x+2 >= 0
* 4x+5 >= 0
* x<= 10
* We formulate this problem as a convex second order cone constraint, by
* regarding the cost as the square of geometric mean between 2x+3, 3x+2 and
* 4x+5. The optimal is x* = 10.
*/
class MaximizeGeometricMeanTrivialProblem2 {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(MaximizeGeometricMeanTrivialProblem2)
MaximizeGeometricMeanTrivialProblem2();
const MathematicalProgram& prog() const { return *prog_; }
void CheckSolution(const MathematicalProgramResult& result, double tol);
private:
std::unique_ptr<MathematicalProgram> prog_;
symbolic::Variable x_;
};
/**
* Tests maximizing geometric mean through second order cone constraint.
* Given some points p₁, p₂, ..., pₖ ∈ ℝⁿ, find the smallest ellipsoid (centered
* at the origin, and aligned with the axes) such that the ellipsoid contains
* all these points.
* This problem can be formulated as
* max a(0) * a(1) * ... * a(n-1)
* s.t pᵢᵀ diag(a) * pᵢ ≤ 1 for all i.
* a(j) > 0
*/
class SmallestEllipsoidCoveringProblem {
public:
// p.col(i) is the point pᵢ.
explicit SmallestEllipsoidCoveringProblem(
const Eigen::Ref<const Eigen::MatrixXd>& p);
virtual ~SmallestEllipsoidCoveringProblem() {}
const MathematicalProgram& prog() const { return *prog_; }
void CheckSolution(const MathematicalProgramResult& result, double tol) const;
protected:
const VectorX<symbolic::Variable>& a() const { return a_; }
private:
// CheckSolution() already checks if the result is successful, and if all the
// points are within the ellipsoid, with at least one point on the boundary
// of the ellipsoid. CheckSolutionExtra can do extra checks for each specific
// problem.
virtual void CheckSolutionExtra(const MathematicalProgramResult&,
double) const {}
std::unique_ptr<MathematicalProgram> prog_;
VectorX<symbolic::Variable> a_;
Eigen::MatrixXd p_;
};
class SmallestEllipsoidCoveringProblem1
: public SmallestEllipsoidCoveringProblem {
public:
SmallestEllipsoidCoveringProblem1();
~SmallestEllipsoidCoveringProblem1() override {}
private:
void CheckSolutionExtra(const MathematicalProgramResult& result,
double tol) const override;
};
void SolveAndCheckSmallestEllipsoidCoveringProblems(
const SolverInterface& solver, double tol);
/**
* Computes the minimal distance to a point from a sphere.
* This problem has a quadratic cost and Lorentz cone constraint.
* min (x - pt)²
* s.t |x - center| <= radius
*/
template <int Dim>
class MinimalDistanceFromSphereProblem {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(MinimalDistanceFromSphereProblem);
/** @param with_linear_cost If set to true, we add the quadratic cost as the
* sum of a quadratic and a linear cost. Otherwise we add it as a single
* quadratic cost.
*/
MinimalDistanceFromSphereProblem(const Eigen::Matrix<double, Dim, 1>& pt,
const Eigen::Matrix<double, Dim, 1>& center,
double radius, bool with_linear_cost)
: prog_{},
x_{prog_.NewContinuousVariables<Dim>()},
pt_{pt},
center_{center},
radius_{radius} {
if (with_linear_cost) {
prog_.AddQuadraticCost(2 * Eigen::Matrix<double, Dim, Dim>::Identity(),
Eigen::Matrix<double, Dim, 1>::Zero(),
0.5 * pt_.squaredNorm(), x_);
prog_.AddLinearCost(-2 * pt_, 0.5 * pt_.squaredNorm(), x_);
} else {
prog_.AddQuadraticErrorCost(Eigen::Matrix<double, Dim, Dim>::Identity(),
pt_, x_);
}
VectorX<symbolic::Expression> lorentz_cone_expr(Dim + 1);
lorentz_cone_expr(0) = radius_;
lorentz_cone_expr.tail(Dim) = x_ - center_;
prog_.AddLorentzConeConstraint(lorentz_cone_expr);
}
MathematicalProgram* get_mutable_prog() { return &prog_; }
void SolveAndCheckSolution(const SolverInterface& solver, double tol) const {
if (solver.available()) {
MathematicalProgramResult result;
solver.Solve(prog_, {}, {}, &result);
EXPECT_TRUE(result.is_success());
const Eigen::Matrix<double, Dim, 1> x_sol = result.GetSolution(x_);
// If pt is inside the sphere, then the optimal solution is x=pt.
if ((pt_ - center_).norm() <= radius_) {
EXPECT_TRUE(CompareMatrices(x_sol, pt_, tol));
EXPECT_NEAR(result.get_optimal_cost(), 0, tol);
} else {
// pt should be the intersection of the ray from center to pt, and the
// sphere surface.
Eigen::Matrix<double, Dim, 1> ray = pt_ - center_;
EXPECT_TRUE(CompareMatrices(
x_sol, center_ + radius_ * (ray.normalized()), tol));
}
}
}
private:
MathematicalProgram prog_;
Eigen::Matrix<symbolic::Variable, Dim, 1> x_;
Eigen::Matrix<double, Dim, 1> pt_;
Eigen::Matrix<double, Dim, 1> center_;
double radius_;
};
void TestSocpDualSolution1(const SolverInterface& solver,
const SolverOptions& solver_options, double tol);
// @param rotated_lorentz_cone_with_coeffcieint_two. Set this to true if this
// solver has a coefficient 2 on the rotated Lorentz cone constraint as 2*x₁x₂
// >= x₃² + ... + xₙ² (like in Mosek). Set this to false if this solver doesn't
// have a coefficient 2 on the rotated Lorentz cone constraint, as x₁x₂
// >= x₃² + ... + xₙ²
void TestSocpDualSolution2(const SolverInterface& solver,
const SolverOptions& solver_options, double tol,
bool rotated_lorentz_cone_with_coefficient_two);
} // namespace test
} // namespace solvers
} // namespace drake