DpleInternal::DpleInternal(const std::map<std::string, std::vector<Sparsity> > &st,
int nrhs, bool transp) :
nrhs_(nrhs), transp_(transp) {
// set default options
setOption("name", "unnamed_dple_solver"); // name of the function
addOption("const_dim", OT_BOOLEAN, true, "Assume constant dimension of P");
addOption("pos_def", OT_BOOLEAN, false, "Assume P positive definite");
addOption("error_unstable", OT_BOOLEAN, false,
"Throw an exception when it is detected that Product(A_i, i=N..1) "
"has eigenvalues greater than 1-eps_unstable");
addOption("eps_unstable", OT_REAL, 1e-4, "A margin for unstability detection");
st_.resize(Dple_STRUCT_NUM);
for (std::map<std::string, std::vector<Sparsity> >::const_iterator i=st.begin();
i!=st.end(); ++i) {
if (i->first=="a") {
st_[Dple_STRUCT_A]=i->second;
} else if (i->first=="v") {
st_[Dple_STRUCT_V]=i->second;
} else {
casadi_error("Unrecognized field in Dple structure: " << i->first);
}
}
if (nrhs_==1) {
ischeme_ = IOScheme(SCHEME_DPLEInput);
oscheme_ = IOScheme(SCHEME_DPLEOutput);
}
}
void SdqpToSdp::init() {
// Initialize the base classes
SdqpSolverInternal::init();
cholesky_ = LinearSolver("cholesky", "csparsecholesky", st_[SDQP_STRUCT_H]);
MX g_socp = MX::sym("x", cholesky_.getFactorizationSparsity(true));
MX h_socp = MX::sym("h", n_);
MX f_socp = sqrt(inner_prod(h_socp, h_socp));
MX en_socp = 0.5/f_socp;
MX f_sdqp = MX::sym("f", input(SDQP_SOLVER_F).sparsity());
MX g_sdqp = MX::sym("g", input(SDQP_SOLVER_G).sparsity());
std::vector<MX> fi(n_+1);
MX znp = MX(n_+1, n_+1);
for (int k=0;k<n_;++k) {
MX gk = vertcat(g_socp(ALL, k), MX(1, 1));
MX fk = -blockcat(znp, gk, gk.T(), MX(1, 1));
// TODO(Joel): replace with ALL
fi.push_back(diagcat(f_sdqp(ALL, Slice(f_sdqp.size1()*k, f_sdqp.size1()*(k+1))), fk));
}
MX fin = en_socp*DMatrix::eye(n_+2);
fin(n_, n_+1) = en_socp;
fin(n_+1, n_) = en_socp;
fi.push_back(diagcat(DMatrix(f_sdqp.size1(), f_sdqp.size1()), -fin));
MX h0 = vertcat(h_socp, DMatrix(1, 1));
MX g = blockcat(f_socp*DMatrix::eye(n_+1), h0, h0.T(), f_socp);
g = diagcat(g_sdqp, g);
Dict opts;
opts["input_scheme"] = IOScheme("g_socp", "h_socp", "f_sdqp", "g_sdqp");
opts["output_scheme"] = IOScheme("f", "g");
mapping_ = MXFunction("mapping", make_vector(g_socp, h_socp, f_sdqp, g_sdqp),
make_vector(horzcat(fi), g), opts);
Dict options;
if (hasSetOption(optionsname())) options = getOption(optionsname());
// Create an SdpSolver instance
solver_ = SdpSolver("sdpsolver", getOption(solvername()),
make_map("g", mapping_.output("g").sparsity(),
"f", mapping_.output("f").sparsity(),
"a", horzcat(input(SDQP_SOLVER_A).sparsity(),
Sparsity(nc_, 1))),
options);
solver_.input(SDP_SOLVER_C).at(n_)=1;
// Output arguments
obuf_.resize(SDQP_SOLVER_NUM_OUT);
output(SDQP_SOLVER_X) = DMatrix::zeros(n_, 1);
std::vector<int> r = range(input(SDQP_SOLVER_G).size1());
output(SDQP_SOLVER_P) = solver_.output(SDP_SOLVER_P).isempty() ? DMatrix() :
solver_.output(SDP_SOLVER_P)(r, r);
output(SDQP_SOLVER_DUAL) = solver_.output(SDP_SOLVER_DUAL).isempty() ? DMatrix() :
solver_.output(SDP_SOLVER_DUAL)(r, r);
output(SDQP_SOLVER_COST) = 0.0;
output(SDQP_SOLVER_DUAL_COST) = 0.0;
output(SDQP_SOLVER_LAM_X) = DMatrix::zeros(n_, 1);
output(SDQP_SOLVER_LAM_A) = DMatrix::zeros(nc_, 1);
}
// Constructor
StabilizedQpSolverInternal::
StabilizedQpSolverInternal(const std::map<std::string, Sparsity> &st) {
st_.resize(QP_STRUCT_NUM);
for (std::map<std::string, Sparsity>::const_iterator i=st.begin(); i!=st.end(); ++i) {
if (i->first=="a") {
st_[QP_STRUCT_A]=i->second;
} else if (i->first=="h") {
st_[QP_STRUCT_H]=i->second;
} else {
casadi_error("Unrecognized field in QP structure: " << i->first);
}
}
// Get structure
const Sparsity& A = st_[QP_STRUCT_A];
const Sparsity& H = st_[QP_STRUCT_H];
// Dimensions
n_ = H.size2();
nc_ = A.isNull() ? 0 : A.size1();
addOption("defaults_recipes", OT_STRINGVECTOR, GenericType(), "",
"qp", true);
// Check consistency
casadi_assert_message(A.isNull() || A.size2()==n_,
"Got incompatible dimensions. min x'Hx + G'x s.t. LBA <= Ax <= UBA :"
<< std::endl <<
"H: " << H.dimString() << " - A: " << A.dimString() << std::endl <<
"We need: H.size2()==A.size2()" << std::endl);
casadi_assert_message(H.issymmetric(),
"Got incompatible dimensions. min x'Hx + G'x" << std::endl <<
"H: " << H.dimString() <<
"We need H square & symmetric" << std::endl);
// IO sparsities
Sparsity x_sparsity = Sparsity::dense(n_, 1);
Sparsity a_sparsity = Sparsity::dense(nc_, 1);
// Input arguments
ibuf_.resize(STABILIZED_QP_SOLVER_NUM_IN);
input(STABILIZED_QP_SOLVER_X0) = DMatrix::zeros(x_sparsity);
input(STABILIZED_QP_SOLVER_H) = DMatrix::zeros(H);
input(STABILIZED_QP_SOLVER_G) = DMatrix::zeros(x_sparsity);
input(STABILIZED_QP_SOLVER_A) = DMatrix::zeros(A);
input(STABILIZED_QP_SOLVER_LBA) = -DMatrix::inf(a_sparsity);
input(STABILIZED_QP_SOLVER_UBA) = DMatrix::inf(a_sparsity);
input(STABILIZED_QP_SOLVER_LBX) = -DMatrix::inf(x_sparsity);
input(STABILIZED_QP_SOLVER_UBX) = DMatrix::inf(x_sparsity);
input(STABILIZED_QP_SOLVER_MUR) = 0.0;
input(STABILIZED_QP_SOLVER_MUE) = DMatrix::zeros(a_sparsity);
input(STABILIZED_QP_SOLVER_MU) = DMatrix::zeros(a_sparsity);
// Output arguments
obuf_.resize(QP_SOLVER_NUM_OUT);
output(QP_SOLVER_X) = DMatrix::zeros(x_sparsity);
output(QP_SOLVER_COST) = 0.0;
output(QP_SOLVER_LAM_X) = DMatrix::zeros(x_sparsity);
output(QP_SOLVER_LAM_A) = DMatrix::zeros(a_sparsity);
// IO scheme
ischeme_ = IOScheme(SCHEME_StabilizedQpSolverInput);
oscheme_ = IOScheme(SCHEME_QpSolverOutput);
}
void LiftingLrDpleInternal::init() {
form_ = getOptionEnumValue("form");
// Initialize the base classes
LrDpleInternal::init();
casadi_assert_message(!pos_def_,
"pos_def option set to True: Solver only handles the indefinite case.");
casadi_assert_message(const_dim_,
"const_dim option set to False: Solver only handles the True case.");
// We will construct an MXFunction to facilitate the calculation of derivatives
MX As = MX::sym("As", input(LR_DLE_A).sparsity());
MX Vs = MX::sym("Vs", input(LR_DLE_V).sparsity());
MX Cs = MX::sym("Cs", input(LR_DLE_C).sparsity());
MX Hs = MX::sym("Hs", input(LR_DLE_H).sparsity());
n_ = A_[0].size1();
// Chop-up the arguments
std::vector<MX> As_ = horzsplit(As, n_);
std::vector<MX> Vs_ = horzsplit(Vs, V_[0].size2());
std::vector<MX> Cs_ = horzsplit(Cs, V_[0].size2());
std::vector<MX> Hs_;
if (with_H_) {
Hs_ = horzsplit(Hs, Hsi_);
}
MX A;
if (K_==1) {
A = As;
} else {
if (form_==0) {
MX AL = diagcat(vector_slice(As_, range(As_.size()-1)));
MX AL2 = horzcat(AL, MX(AL.size1(), As_[0].size2()));
MX AT = horzcat(MX(As_[0].size1(), AL.size2()), As_.back());
A = vertcat(AT, AL2);
} else {
MX AL = diagcat(reverse(vector_slice(As_, range(As_.size()-1))));
MX AL2 = horzcat(MX(AL.size1(), As_[0].size2()), AL);
MX AT = horzcat(As_.back(), MX(As_[0].size1(), AL.size2()));
A = vertcat(AL2, AT);
}
}
MX V;
MX C;
MX H;
if (form_==0) {
V = diagcat(Vs_.back(), diagcat(vector_slice(Vs_, range(Vs_.size()-1))));
if (with_C_) {
C = diagcat(Cs_.back(), diagcat(vector_slice(Cs_, range(Cs_.size()-1))));
}
} else {
V = diagcat(diagcat(reverse(vector_slice(Vs_, range(Vs_.size()-1)))), Vs_.back());
if (with_C_) {
C = diagcat(diagcat(reverse(vector_slice(Cs_, range(Cs_.size()-1)))), Cs_.back());
}
}
if (with_H_) {
H = diagcat(form_==0? Hs_ : reverse(Hs_));
}
// QP solver options
Dict options;
if (hasSetOption(optionsname())) {
options = getOption(optionsname());
}
options["Hs"] = Hss_;
// Create an LrDleSolver instance
solver_ = LrDleSolver("solver", getOption(solvername()),
make_map("a", A.sparsity(),
"v", V.sparsity(),
"c", C.sparsity(),
"h", H.sparsity()), options);
std::vector<MX> v_in(LR_DPLE_NUM_IN);
v_in[LR_DLE_A] = As;
v_in[LR_DLE_V] = Vs;
if (with_C_) {
v_in[LR_DLE_C] = Cs;
}
if (with_H_) {
v_in[LR_DLE_H] = Hs;
}
MX Pf = solver_(make_map("a", A, "v", V, "c", C, "h", H)).at(solver_.outputName(0));
std::vector<MX> Ps = with_H_ ? diagsplit(Pf, Hsi_) : diagsplit(Pf, n_);
if (form_==1) {
Ps = reverse(Ps);
}
f_ = MXFunction(name_, v_in, dpleOut("p", horzcat(Ps)),
make_dict("input_scheme", IOScheme(SCHEME_LR_DPLEInput),
"output_scheme", IOScheme(SCHEME_LR_DPLEOutput)));
Wrapper<LiftingLrDpleInternal>::checkDimensions();
}
// Constructor
IOSchemeVector(const std::vector<T>& d = std::vector<T>(), const IOScheme& s = IOScheme()) : data(d), scheme(s) {}
