-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
csdp_solver.h
130 lines (119 loc) · 3.96 KB
/
csdp_solver.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
#pragma once
#include "drake/common/drake_copyable.h"
#include "drake/solvers/solver_base.h"
namespace drake {
namespace solvers {
/**
* The CSDP solver details after calling Solve() function. The user can call
* MathematicalProgramResult::get_solver_details<CsdpSolver>() to obtain the
* details.
*/
struct CsdpSolverDetails {
/** Refer to the Return Codes section of CSDP 6.2.0 User's Guide for
* explanation on the return code. Some of the common return codes are
*
* 0 Problem is solved to optimality.
* 1 Problem is primal infeasible.
* 2 Problem is dual infeasible.
* 3 Problem solved to near optimality.
* 4 Maximum iterations reached.
*/
int return_code{};
/** The primal objective value. */
double primal_objective{};
/** The dual objective value. */
double dual_objective{};
/**
* CSDP solves a primal problem of the form
*
* max tr(C*X)
* s.t tr(Aᵢ*X) = aᵢ
* X ≽ 0
*
* The dual form is
*
* min aᵀy
* s.t ∑ᵢ yᵢAᵢ - C = Z
* Z ≽ 0
*
* y, Z are the variables for the dual problem.
* y_val, Z_val are the solutions to the dual problem.
*/
Eigen::VectorXd y_val;
Eigen::SparseMatrix<double> Z_val;
};
class CsdpSolver final : public SolverBase {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(CsdpSolver)
/**
* CSDP doesn't accept free variables, namely the problem it solves is in this
* form P1
*
* max tr(C * X)
* s.t tr(Aᵢ*X) = aᵢ
* X ≽ 0.
*
* Notice that the decision variable X has to be in the proper cone X ≽ 0, and
* it doesn't accept free variable (without the conic constraint). On the
* other hand, most real-world applications require free variables, namely
* problems in this form P2
*
* max tr(C * X) + dᵀs
* s.t tr(Aᵢ*X) + bᵢᵀs = aᵢ
* X ≽ 0
* s is free.
*
* In order to remove the free variables, we consider two approaches.
* 1. Replace a free variable s with two variables s = p - q, p ≥ 0, q ≥ 0.
* 2. First write the dual of the problem P2 as D2
*
* min aᵀy
* s.t ∑ᵢ yᵢAᵢ - C = Z
* Z ≽ 0
* Bᵀ * y = d,
*
* where bᵢᵀ is the i'th row of B.
* The last constraint Bᵀ * y = d means y = ŷ + Nt, where Bᵀ * ŷ = d, and N
* is the null space of Bᵀ. Hence, D2 is equivalent to the following
* problem, D3
*
* min aᵀNt + aᵀŷ
* s.t ∑ᵢ tᵢFᵢ - (C -∑ᵢ ŷᵢAᵢ) = Z
* Z ≽ 0,
*
* where Fᵢ = ∑ⱼ NⱼᵢAⱼ. D3 is the dual of the following primal problem P3
* without free variables
*
* max tr((C-∑ᵢ ŷᵢAᵢ)*X̂) + aᵀŷ
* s.t tr(FᵢX̂) = (Nᵀa)(i)
* X̂ ≽ 0.
*
* Then (X, s) = (X̂, B⁻¹(a - tr(Aᵢ X̂))) is the solution to the original
* problem P2.
*
*/
enum RemoveFreeVariableMethod {
kTwoSlackVariables, ///< Approach 1, replace a free variable s as
///< s = y⁺ - y⁻, y⁺ ≥ 0, y⁻ ≥ 0.
kNullspace, ///< Approach 2, reformulate the dual problem by considering
///< the nullspace of the linear constraint in the dual.
};
explicit CsdpSolver(RemoveFreeVariableMethod method = kNullspace);
~CsdpSolver() final;
/// @name Static versions of the instance methods with similar names.
//@{
static SolverId id();
static bool is_available();
static bool is_enabled();
static bool ProgramAttributesSatisfied(const MathematicalProgram&);
//@}
// A using-declaration adds these methods into our class's Doxygen.
using SolverBase::Solve;
using Details = CsdpSolverDetails;
private:
void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&,
const SolverOptions&, MathematicalProgramResult*) const final;
RemoveFreeVariableMethod method_;
};
} // namespace solvers
} // namespace drake