Example #1
0
  void LrDpleToDple::init() {
    // Initialize the base classes
    LrDpleInternal::init();

    MX As = MX::sym("As", input(LR_DPLE_A).sparsity());
    MX Vs = MX::sym("Vs", input(LR_DPLE_V).sparsity());
    MX Cs = MX::sym("Cs", input(LR_DPLE_C).sparsity());
    MX Hs = MX::sym("Hs", input(LR_DPLE_H).sparsity());

    int 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> Hss_ = horzsplit(Hs, Hsi_);

    std::vector<MX> V_(Vs_.size());

    for (int k=0;k<V_.size();++k) {
      V_[k] = mul(Cs_[k], mul(Vs_[k], Cs_[k].T()));
    }

    std::vector<Sparsity> Vsp(Vs_.size());
    for (int k=0;k<V_.size();++k) {
      Vsp[k] = V_[k].sparsity();
    }

    // Solver options
    Dict options;
    if (hasSetOption(optionsname())) {
      options = getOption(optionsname());
    }

    // Create an dplesolver instance
    std::map<std::string, std::vector<Sparsity> > tmp;
    tmp["a"] = A_;
    tmp["v"] = Vsp;
    solver_ = DpleSolver("solver", getOption(solvername()), tmp, options);

    MX P = solver_(make_map("a", horzcat(As_), "v", horzcat(V_))).at("p");
    std::vector<MX> Ps_ = horzsplit(P, n_);

    std::vector<MX> HPH(K_);

    for (int k=0;k<K_;++k) {
      std::vector<MX> hph = horzsplit(Hss_[k], cumsum0(Hs_[k]));

      for (int kk=0;kk<hph.size();++kk) {
        hph[kk] = mul(hph[kk].T(), mul(Ps_[k], hph[kk]));
      }
      HPH[k] = diagcat(hph);
    }


    f_ = MXFunction(name_, lrdpleIn("a", As, "v", Vs, "c", Cs, "h", Hs),
                    lrdpleOut("y", horzcat(HPH)));

    Wrapper<LrDpleToDple>::checkDimensions();
  }
Example #2
0
  void Diagsplit::evalAdj(const std::vector<pv_MX>& adjSeed, const std::vector<pv_MX>& adjSens) {
    int nadj = adjSens.size();
    int nx = offset_.size()-1;

    // Get offsets
    vector<int> offset1;
    offset1.reserve(offset_.size());
    offset1.push_back(0);
    vector<int> offset2;
    offset2.reserve(offset_.size());
    offset2.push_back(0);
    for (std::vector<Sparsity>::const_iterator it=output_sparsity_.begin();
        it!=output_sparsity_.end();
        ++it) {
      offset1.push_back(offset1.back() + it->size1());
      offset2.push_back(offset2.back() + it->size2());
    }

    for (int d=0; d<nadj; ++d) {
      if (adjSens[d][0]!=0) {
        vector<MX> v;
        for (int i=0; i<nx; ++i) {
          MX* x_i = adjSeed[d][i];
          if (x_i!=0) {
            v.push_back(*x_i);
            *x_i = MX();
          } else {
            v.push_back(MX(output_sparsity_[i].shape()));
          }
        }
        adjSens[d][0]->addToSum(diagcat(v));
      }
    }
  }
Example #3
0
  std::vector<Sparsity>
  LrDpleInternal::getSparsity(const std::map<std::string, std::vector<Sparsity> >& st,
                              const std::vector< std::vector<int> > &Hs_) {
    // Chop-up the arguments
    std::vector<Sparsity> As, Vs, Cs, Hs;
    if (st.count("a")) As = st.at("a");
    if (st.count("v")) Vs = st.at("v");
    if (st.count("c")) Cs = st.at("c");
    if (st.count("h")) Hs = st.at("h");

    bool with_H = !Hs.empty();

    int K = As.size();

    Sparsity A;
    if (K==1) {
      A = As[0];
    } else {
      Sparsity AL = diagcat(vector_slice(As, range(As.size()-1)));

      Sparsity AL2 = horzcat(AL, Sparsity(AL.size1(), As[0].size2()));
      Sparsity AT = horzcat(Sparsity(As[0].size1(), AL.size2()), As.back());
      A = vertcat(AT, AL2);
    }

    Sparsity V = diagcat(Vs.back(), diagcat(vector_slice(Vs, range(Vs.size()-1))));
    Sparsity C;

    if (!Cs.empty()) {
      C = diagcat(Cs.back(), diagcat(vector_slice(Cs, range(Cs.size()-1))));
    }
    Sparsity H;
    std::vector<int> Hs_agg;

    std::vector<int> Hi(1, 0);
    if (Hs_.size()>0 && with_H) {
      H = diagcat(Hs.back(), diagcat(vector_slice(Hs, range(Hs.size()-1))));
      casadi_assert(K==Hs_.size());
      for (int k=0;k<K;++k) {
        Hs_agg.insert(Hs_agg.end(), Hs_[k].begin(), Hs_[k].end());
        int sum=0;
        for (int i=0;i<Hs_[k].size();++i) {
          sum+= Hs_[k][i];
        }
        Hi.push_back(Hi.back()+sum);
      }
    }

    Sparsity res = LrDleInternal::getSparsity(make_map("a", A, "v", V, "c", C, "h", H), Hs_agg);

    if (with_H) {
      return diagsplit(res, Hi);
    } else {
      return diagsplit(res, As[0].size2());
    }
  }
Example #4
0
  void Diagcat::evaluateMX(const MXPtrV& input, MXPtrV& output, const MXPtrVV& fwdSeed,
                           MXPtrVV& fwdSens, const MXPtrVV& adjSeed, MXPtrVV& adjSens,
                           bool output_given) {
    int nfwd = fwdSens.size();
    int nadj = adjSeed.size();

    // Non-differentiated output
    if (!output_given) {
      *output[0] = diagcat(getVector(input));
    }

    // Forward sensitivities
    for (int d = 0; d<nfwd; ++d) {
      *fwdSens[d][0] = diagcat(getVector(fwdSeed[d]));
    }

    // Quick return?
    if (nadj==0) return;

    // Get offsets for each row and column
    vector<int> offset1(ndep()+1, 0);
    vector<int> offset2(ndep()+1, 0);
    for (int i=0; i<ndep(); ++i) {
      int ncol = dep(i).sparsity().size2();
      int nrow = dep(i).sparsity().size1();
      offset2[i+1] = offset2[i] + ncol;
      offset1[i+1] = offset1[i] + nrow;
    }

    // Adjoint sensitivities
    for (int d=0; d<nadj; ++d) {
      MX& aseed = *adjSeed[d][0];
      vector<MX> s = diagsplit(aseed, offset1, offset2);
      aseed = MX();
      for (int i=0; i<ndep(); ++i) {
        adjSens[d][i]->addToSum(s[i]);
      }
    }
  }
  void FixedSmithLrDleInternal::init() {
    iter_  = getOption("iter");

    LrDleInternal::init();

    casadi_assert_message(!pos_def_,
      "pos_def option set to True: Solver only handles the indefinite case.");

    MX H = MX::sym("H", H_);
    MX A = MX::sym("A", A_);
    MX C = MX::sym("C", C_);
    MX V = MX::sym("V", V_);

    MX Vs = (V+V.T())/2;

    MX D = with_C_ ? C : DMatrix::eye(A_.size1());


    std::vector<MX> HPH(Hs_.size(), 0);
    std::vector<MX> Hs = with_H_? horzsplit(H, Hi_) : std::vector<MX>();
    MX out = 0;

    for (int i=0;i<iter_;++i) {
      if (with_H_) {
        for (int k=0;k<Hs.size();++k) {
          MX v = mul(D.T(), Hs[k]);
          HPH[k]+= mul(v.T(), mul(Vs, v));
        }
      } else {
        out += mul(D, mul(Vs, D.T()));
      }
      D = mul(A, D);
    }

    std::vector<MX> dle_in(LR_DLE_NUM_IN);
    dle_in[LR_DLE_A] = A;
    dle_in[LR_DLE_V] = V;
    if (with_C_) dle_in[LR_DLE_C] = C;
    if (with_H_) dle_in[LR_DLE_H] = H;

    f_ = MXFunction(dle_in, lrdleOut("y", with_H_? diagcat(HPH): out));
    f_.init();

    Wrapper<FixedSmithLrDleInternal>::checkDimensions();

  }
Example #6
0
  void StabilizedQpToQp::init() {
    // Initialize the base classes
    StabilizedQpSolverInternal::init();

    // Form augmented QP
    Sparsity H_sparsity_qp = diagcat(st_[QP_STRUCT_H], Sparsity::diag(nc_));
    Sparsity A_sparsity_qp = horzcat(st_[QP_STRUCT_A], Sparsity::diag(nc_));
    std::string qp_solver_name = getOption("qp_solver");
    qp_solver_ = QpSolver(qp_solver_name,
                          qpStruct("h", H_sparsity_qp, "a", A_sparsity_qp));

    // Pass options if provided
    if (hasSetOption("qp_solver_options")) {
      Dictionary qp_solver_options = getOption("qp_solver_options");
      qp_solver_.setOption(qp_solver_options);
    }

    // Initialize the QP solver
    qp_solver_.init();
  }
Example #7
0
  void Diagsplit::evalAdj(const std::vector<std::vector<MX> >& aseed,
                          std::vector<std::vector<MX> >& asens) {
    int nadj = asens.size();

    // Get offsets
    vector<int> offset1;
    offset1.reserve(offset_.size());
    offset1.push_back(0);
    vector<int> offset2;
    offset2.reserve(offset_.size());
    offset2.push_back(0);
    for (std::vector<Sparsity>::const_iterator it=output_sparsity_.begin();
        it!=output_sparsity_.end();
        ++it) {
      offset1.push_back(offset1.back() + it->size1());
      offset2.push_back(offset2.back() + it->size2());
    }

    for (int d=0; d<nadj; ++d) {
      asens[d][0] += diagcat(aseed[d]);
    }
  }
  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::sparse(AL.size1(), As_[0].size2()));
        MX AT = horzcat(MX::sparse(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::sparse(AL.size1(), As_[0].size2()), AL);
        MX AT = horzcat(As_.back(), MX::sparse(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_));
    }

    // Create an LrDleSolver instance
    solver_ = LrDleSolver(getOption(solvername()),
                          lrdleStruct("a", A.sparsity(),
                                      "v", V.sparsity(),
                                      "c", C.sparsity(),
                                      "h", H.sparsity()));
    solver_.setOption("Hs", Hss_);
    if (hasSetOption(optionsname())) solver_.setOption(getOption(optionsname()));
    solver_.init();

    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;
    }

    std::vector<MX> Pr = solver_.call(lrdpleIn("a", A, "v", V, "c", C, "h", H));

    MX Pf = Pr[0];

    std::vector<MX> Ps = with_H_ ? diagsplit(Pf, Hsi_) : diagsplit(Pf, n_);

    if (form_==1) {
      Ps = reverse(Ps);
    }

    f_ = MXFunction(v_in, dpleOut("p", horzcat(Ps)));
    f_.setInputScheme(SCHEME_LR_DPLEInput);
    f_.setOutputScheme(SCHEME_LR_DPLEOutput);
    f_.init();

    Wrapper::checkDimensions();

  }
  void SimpleIndefDleInternal::init() {

    DleInternal::init();

    casadi_assert_message(!pos_def_,
      "pos_def option set to True: Solver only handles the indefinite case.");


    n_ = A_.size1();

    MX As = MX::sym("A", A_);
    MX Vs = MX::sym("V", V_);
    MX Cs = MX::sym("C", C_);
    MX Hs = MX::sym("H", H_);

    MX Vss = (Vs+Vs.T())/2;
    if (with_C_) Vss = mul(mul(Cs, Vss), Cs.T());

    MX A_total = DMatrix::eye(n_*n_) - kron(As,As);

    // Should be treated by solve node
    MX Pf = solve(A_total, vec(Vss), getOption("linear_solver"));

    std::vector<MX> v_in;
    v_in.push_back(As);
    v_in.push_back(Vs);
    v_in.push_back(Cs);

    MX P = reshape(Pf,n_,n_);

    std::vector<MX> HPH;

    if (with_H_) {
      std::vector<MX> H = horzsplit(Hs,Hi_);

      for (int k=0;k<H.size();++k) {
        HPH.push_back(mul(H[k].T(),mul(P,H[k])));
      }
    }

    std::vector<MX> dle_in(DLE_NUM_IN);
    dle_in[DLE_A] = As;
    dle_in[DLE_V] = Vs;
    if (with_C_) dle_in[DLE_C] = Cs;
    if (with_H_) dle_in[DLE_H] = Hs;

    f_ = MXFunction(dle_in,dleOut("p",with_H_? diagcat(HPH) : P(output().sparsity())));

    f_.init();

    casadi_assert(nOut()==f_.nOut());
    for (int i=0;i<nIn();++i) {
      casadi_assert_message(input(i).sparsity()==f_.input(i).sparsity(),
        "Sparsity mismatch for input " << i << ":" <<
        input(i).dimString() << " <-> " << f_.input(i).dimString() << ".");
    }
    for (int i=0;i<nOut();++i) {
      casadi_assert_message(output(i).sparsity()==f_.output(i).sparsity(),
        "Sparsity mismatch for output " << i << ":" <<
        output(i).dimString() << " <-> " << f_.output(i).dimString() << ".");
    }
  }
Example #10
0
  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);
  }
Example #11
0
  void LrDpleInternal::init() {

    const_dim_ = getOption("const_dim");
    pos_def_ = getOption("pos_def");
    error_unstable_ = getOption("error_unstable");
    eps_unstable_ = getOption("eps_unstable");
    Hs_ = getOption("Hs");

    casadi_assert(const_dim_);

    A_ = st_[LR_Dple_STRUCT_A];
    casadi_assert(A_.size()>0);
    int n = A_[0].size1();

    V_ = st_[LR_Dple_STRUCT_V];
    C_ = st_[LR_Dple_STRUCT_C];
    H_ = st_[LR_Dple_STRUCT_H];

    with_H_ = true;

    if (H_.empty()) {
      with_H_ = false;
    }



    with_C_ = true;
    if (C_.empty()) {
      with_C_ = false;
    }

    if (with_H_) {
      casadi_assert_message(A_.size()==H_.size(),
                          "A and H arguments must be of same length, but got "
                          << A_.size() << " and " << H_.size() << ".");
      casadi_assert_message(H_.size()==Hs_.size(),
                          "H and Hs arguments must be of same length, but got "
                          << H_.size() << " and " << Hs_.size() << ".");

      Hsi_.resize(1, 0);
      for (int k=0;k<H_.size();++k) {

        // Default hs: [H.size2()]
        if (Hs_[k].size()==0) {
          Hs_[k].push_back(H_[k].size2());
        }

        // Assert that sum of Hs entries match up to H.size2()
        double sum = 0;
        for (int i=0;i<Hs_[k].size();++i) {
          sum += Hs_[k][i];
        }

        Hsi_.push_back(Hsi_.back()+sum);

        casadi_assert_message(H_[k].size2()==sum,
                       "Number of columns in H @ k = " << k << "  (" << H_[k].size2() << "),"
                       << "must match sum(Hs[k]): " << sum << ".");
      }

    }


    // Dimension sanity checks
    casadi_assert_message(A_.size()==V_.size(), "A and V arguments must be of same length, but got "
                          << A_.size() << " and " << V_.size() << ".");
    K_ = A_.size();

    int m = with_C_? V_[0].size1(): n;

    for (int k=0;k<K_;++k) {
      casadi_assert_message(V_[k].issymmetric(), "V_i must be symmetric but got "
                            << V_[k].dimString() << " for i = " << k << ".");
      if (with_C_) {
        casadi_assert_message(n==C_[k].size1(), "Number of rows in C ("
                              << C_[k].size1() << ") must match dimension of square A @ k = "
                              << k << " (" << n << ")" << ".");
        casadi_assert_message(m==C_[k].size2(), "Number of columns in C ("
                              << C_[k].size2() << ") must match dimension of symmetric V @ k = "
                              << k << " (" << m << ")" << ".");
      } else {
          casadi_assert_message(A_[k].size1()==V_[k].size1(), "First dimension of A ("
                            << A_[k].size1() << ") must match dimension of symmetric V @ k = "
                            << k << " (" << V_[k].size1() << ")" << ".");
      }
    }



    if (const_dim_) {
      int n = A_[0].size1();
       for (int k=1;k<K_;++k) {
         casadi_assert_message(A_[k].size1()==n, "You have set const_dim option, but found "
                               "an A_i with dimension ( " << A_[k].dimString()
                               << " ) deviating from n = " << n << " at i = " << k << ".");
      }
    }

    Hss_.resize(0);

    if (Hs_.size()>0) {
      for (int k=0;k<K_;++k) {
        Hss_.insert(Hss_.end(), Hs_[k].begin(), Hs_[k].end());
      }
    }

    // Allocate inputs
    ibuf_.resize(LR_DPLE_NUM_IN);

    if (const_dim_) {
      input(LR_DPLE_A)  = DMatrix::zeros(horzcat(A_));

      if (with_C_) {
        input(LR_DPLE_C)  = DMatrix::zeros(horzcat(C_));
      }
      input(LR_DPLE_V)  = DMatrix::zeros(horzcat(V_));
      if (with_H_) {
        input(LR_DPLE_H)  = DMatrix::zeros(horzcat(H_));
      }
    } else {
      input(LR_DPLE_A)  = DMatrix::zeros(diagcat(A_));
    }

    /**for (int i=0;i<nrhs_;++i) {
      if (const_dim_) {
        input(1+i)  = DMatrix::zeros(horzcat(V_));
      } else {
        input(1+i)  = DMatrix::zeros(diagcat(V_));
      }
    }*/

    // Allocate outputs
    std::map<std::string, std::vector<Sparsity> > tmp;
    tmp["a"] = st_[LR_Dple_STRUCT_A];
    tmp["v"] = st_[LR_Dple_STRUCT_V];
    tmp["c"] = st_[LR_Dple_STRUCT_C];
    tmp["h"] = st_[LR_Dple_STRUCT_H];
    std::vector<Sparsity> P = getSparsity(tmp, Hs_);
    obuf_.resize(nrhs_*LR_DPLE_NUM_OUT);
    for (int i=0;i<nrhs_;++i) {
      if (const_dim_) {
        output(i) = DMatrix::zeros(horzcat(P));
      } else {
        output(i) = DMatrix::zeros(diagcat(P));
      }
    }

    FunctionInternal::init();

  }
Example #12
0
  void DpleInternal::init() {

    const_dim_ = getOption("const_dim");
    pos_def_ = getOption("pos_def");
    error_unstable_ = getOption("error_unstable");
    eps_unstable_ = getOption("eps_unstable");
    A_ = st_[Dple_STRUCT_A];
    V_ = st_[Dple_STRUCT_V];

    // Dimension sanity checks
    casadi_assert_message(A_.size()==V_.size(), "A and V arguments must be of same length, but got "
                          << A_.size() << " and " << V_.size() << ".");
    K_ = A_.size();
    for (int k=0;k<K_;++k) {
      casadi_assert_message(V_[k].issymmetric(), "V_i must be symmetric but got "
                            << V_[k].dimString() << " for i = " << k << ".");

      casadi_assert_message(A_[k].size1()==V_[k].size1(), "First dimension of A ("
                            << A_[k].size1() << ") must match dimension of symmetric V_i ("
                            << V_[k].size1() << ")" << " for i = " << k << ".");
    }

    if (const_dim_) {
      int n = A_[0].size1();
       for (int k=1;k<K_;++k) {
         casadi_assert_message(A_[k].size1()==n, "You have set const_dim option, but found "
                               "an A_i with dimension ( " << A_[k].dimString()
                               << " ) deviating from n = " << n << " at i = " << k << ".");
      }
    }

    // Allocate inputs
    ibuf_.resize(1+nrhs_);

    if (const_dim_) {
      input(0)  = DMatrix::zeros(horzcat(A_));
    } else {
      input(0)  = DMatrix::zeros(diagcat(A_));
    }

    for (int i=0;i<nrhs_;++i) {
      if (const_dim_) {
        input(1+i)  = DMatrix::zeros(horzcat(V_));
      } else {
        input(1+i)  = DMatrix::zeros(diagcat(V_));
      }
    }

    // Allocate outputs
    std::map<std::string, std::vector<Sparsity> > tmp;
    tmp["a"] = A_;
    tmp["v"] = V_;
    std::vector<Sparsity> P = LrDpleInternal::getSparsity(tmp);
    obuf_.resize(nrhs_);
    for (int i=0;i<nrhs_;++i) {
      if (const_dim_) {
        output(i) = DMatrix::zeros(horzcat(P));
      } else {
        output(i) = DMatrix::zeros(diagcat(P));
      }
    }

    FunctionInternal::init();

  }