int main(void)
{
cs *T, *A, *B;
int m = 100, n = 3;
T = cs_load(stdin);
A = cs_compress(T);
B = blkdiag(A, m, n);
printf("B is %d x %d\n", B->m, B->n);
sparseprint(B);
cs_spfree(T);
cs_spfree(A);
cs_spfree(B);
// Test the block diagonal feature
// cs *T, *L, *Lt, *A, *X, *B;
// css *S;
// T = cs_load(stdin);
// L = cs_compress(T);
// Lt = cs_transpose(L, 1);
// A = cs_multiply(L, Lt);
// cs_spfree(T);
// sparseprint(A);
// S = cs_schol(0, A);
// B = speye(A->n);
// X = mldivide_chol(A, S, B);
// sparseprint(X);
}
Sparsity blkdiag(const Sparsity &a, const Sparsity &b) {
std::vector<Sparsity> v;
v.push_back(a);
v.push_back(b);
return blkdiag(v);
}
Diagcat::Diagcat(const std::vector<MX>& x) : Concat(x) {
// Construct the sparsity
casadi_assert(!x.empty());
std::vector<Sparsity> sp;
for (int i=0;i<x.size(); ++i) {
sp.push_back(x[i].sparsity());
}
setSparsity(blkdiag(sp));
}
void SimpleIndefDpleInternal::init() {
DpleInternal::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.");
n_ = A_[0].size1();
MX As = MX::sym("A", n_, K_*n_);
MX Vs = MX::sym("V", n_, K_*n_);
std::vector< MX > Vss = horzsplit(Vs, n_);
std::vector< MX > Ass = horzsplit(As, n_);
for (int k=0;k<K_;++k) {
Vss[k]=(Vss[k]+Vss[k].T())/2;
}
std::vector< MX > AA_list(K_);
for (int k=0;k<K_;++k) {
AA_list[k] = kron(Ass[k], Ass[k]);
}
MX AA = blkdiag(AA_list);
MX A_total = DMatrix::eye(n_*n_*K_) -
vertcat(AA(range(K_*n_*n_-n_*n_, K_*n_*n_), range(K_*n_*n_)),
AA(range(K_*n_*n_-n_*n_), range(K_*n_*n_)));
std::vector<MX> Vss_shift;
Vss_shift.push_back(Vss.back());
Vss_shift.insert(Vss_shift.end(), Vss.begin(), Vss.begin()+K_-1);
MX Pf = solve(A_total, vec(horzcat(Vss_shift)), getOption("linear_solver"));
MX P = reshape(Pf, n_, K_*n_);
std::vector<MX> v_in;
v_in.push_back(As);
v_in.push_back(Vs);
f_ = MXFunction(v_in, P);
f_.setInputScheme(SCHEME_DPLEInput);
f_.setOutputScheme(SCHEME_DPLEOutput);
f_.init();
}
void QcqpToSocp::init() {
// Initialize the base classes
QcqpSolverInternal::init();
// Collection of sparsities that will make up SOCP_SOLVER_G
std::vector<Sparsity> socp_g;
// Allocate Cholesky solvers
cholesky_.push_back(LinearSolver("csparsecholesky", st_[QCQP_STRUCT_H]));
for (int i=0;i<nq_;++i) {
cholesky_.push_back(
LinearSolver("csparsecholesky",
DMatrix(st_[QCQP_STRUCT_P])(range(i*n_, (i+1)*n_), ALL).sparsity()));
}
for (int i=0;i<nq_+1;++i) {
// Initialize Cholesky solve
cholesky_[i].init();
// Harvest Cholsesky sparsity patterns
// Note that we add extra scalar to make room for the epigraph-reformulation variable
socp_g.push_back(blkdiag(
cholesky_[i].getFactorizationSparsity(false), Sparsity::dense(1, 1)));
}
// Create an SocpSolver instance
solver_ = SocpSolver(getOption(solvername()),
socpStruct("g", horzcat(socp_g),
"a", horzcat(input(QCQP_SOLVER_A).sparsity(),
Sparsity::sparse(nc_, 1))));
//solver_.setQCQPOptions();
if (hasSetOption(optionsname())) solver_.setOption(getOption(optionsname()));
std::vector<int> ni(nq_+1);
for (int i=0;i<nq_+1;++i) {
ni[i] = n_+1;
}
solver_.setOption("ni", ni);
// Initialize the SocpSolver
solver_.init();
}
void SDPSDQPInternal::init() {
// Initialize the base classes
SdqpSolverInternal::init();
cholesky_ = LinearSolver("csparsecholesky", st_[SDQP_STRUCT_H]);
cholesky_.init();
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::sparse(n_+1, n_+1);
for (int k=0;k<n_;++k) {
MX gk = vertcat(g_socp(ALL, k), DMatrix::sparse(1, 1));
MX fk = -blockcat(znp, gk, gk.T(), DMatrix::sparse(1, 1));
// TODO(Joel): replace with ALL
fi.push_back(blkdiag(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(blkdiag(DMatrix::sparse(f_sdqp.size1(), f_sdqp.size1()), -fin));
MX h0 = vertcat(h_socp, DMatrix::sparse(1, 1));
MX g = blockcat(f_socp*DMatrix::eye(n_+1), h0, h0.T(), f_socp);
g = blkdiag(g_sdqp, g);
IOScheme mappingIn("g_socp", "h_socp", "f_sdqp", "g_sdqp");
IOScheme mappingOut("f", "g");
mapping_ = MXFunction(mappingIn("g_socp", g_socp, "h_socp", h_socp,
"f_sdqp", f_sdqp, "g_sdqp", g_sdqp),
mappingOut("f", horzcat(fi), "g", g));
mapping_.init();
// Create an sdpsolver instance
std::string sdpsolver_name = getOption("sdp_solver");
sdpsolver_ = SdpSolver(sdpsolver_name,
sdpStruct("g", mapping_.output("g").sparsity(),
"f", mapping_.output("f").sparsity(),
"a", horzcat(input(SDQP_SOLVER_A).sparsity(),
Sparsity::sparse(nc_, 1))));
if (hasSetOption("sdp_solver_options")) {
sdpsolver_.setOption(getOption("sdp_solver_options"));
}
// Initialize the SDP solver
sdpsolver_.init();
sdpsolver_.input(SDP_SOLVER_C).at(n_)=1;
// Output arguments
setNumOutputs(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) = sdpsolver_.output(SDP_SOLVER_P).isEmpty() ? DMatrix() :
sdpsolver_.output(SDP_SOLVER_P)(r, r);
output(SDQP_SOLVER_DUAL) = sdpsolver_.output(SDP_SOLVER_DUAL).isEmpty() ? DMatrix() :
sdpsolver_.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);
}
void SdpSolverInternal::init() {
// Call the init method of the base class
FunctionInternal::init();
calc_p_ = getOption("calc_p");
calc_dual_ = getOption("calc_dual");
print_problem_ = getOption("print_problem");
// Find aggregate sparsity pattern
Sparsity aggregate = input(SDP_SOLVER_G).sparsity();
for (int i=0;i<n_;++i) {
aggregate = aggregate + input(SDP_SOLVER_F)(ALL, Slice(i*m_, (i+1)*m_)).sparsity();
}
// Detect block diagonal structure in this sparsity pattern
std::vector<int> p;
std::vector<int> r;
nb_ = aggregate.stronglyConnectedComponents(p, r);
block_boundaries_.resize(nb_+1);
std::copy(r.begin(), r.begin()+nb_+1, block_boundaries_.begin());
block_sizes_.resize(nb_);
for (int i=0;i<nb_;++i) {
block_sizes_[i]=r[i+1]-r[i];
}
// Make a mapping function from dense blocks to inversely-permuted block diagonal P
std::vector< SX > full_blocks;
for (int i=0;i<nb_;++i) {
full_blocks.push_back(SX::sym("block", block_sizes_[i], block_sizes_[i]));
}
Pmapper_ = SXFunction(full_blocks, blkdiag(full_blocks)(lookupvector(p, p.size()),
lookupvector(p, p.size())));
Pmapper_.init();
if (nb_>0) {
// Make a mapping function from (G, F) -> (G[p, p]_j, F_i[p, p]j)
SX G = SX::sym("G", input(SDP_SOLVER_G).sparsity());
SX F = SX::sym("F", input(SDP_SOLVER_F).sparsity());
std::vector<SX> in;
in.push_back(G);
in.push_back(F);
std::vector<SX> out((n_+1)*nb_);
for (int j=0;j<nb_;++j) {
out[j] = G(p, p)(Slice(r[j], r[j+1]), Slice(r[j], r[j+1]));
}
for (int i=0;i<n_;++i) {
SX Fi = F(ALL, Slice(i*m_, (i+1)*m_))(p, p);
for (int j=0;j<nb_;++j) {
out[(i+1)*nb_+j] = Fi(Slice(r[j], r[j+1]), Slice(r[j], r[j+1]));
}
}
mapping_ = SXFunction(in, out);
mapping_.init();
}
// Output arguments
setNumOutputs(SDP_SOLVER_NUM_OUT);
output(SDP_SOLVER_X) = DMatrix::zeros(n_, 1);
output(SDP_SOLVER_P) = calc_p_? DMatrix(Pmapper_.output().sparsity(), 0) : DMatrix();
output(SDP_SOLVER_DUAL) = calc_dual_? DMatrix(Pmapper_.output().sparsity(), 0) : DMatrix();
output(SDP_SOLVER_COST) = 0.0;
output(SDP_SOLVER_DUAL_COST) = 0.0;
output(SDP_SOLVER_LAM_X) = DMatrix::zeros(n_, 1);
output(SDP_SOLVER_LAM_A) = DMatrix::zeros(nc_, 1);
}
