Exemplo n.º 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();
  }
  void CondensingIndefDpleInternal::init() {
    // Initialize the base classes
    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", horzcat(A_));
    MX Vs = MX::sym("V", horzcat(V_));

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

    MX R = MX::zeros(n_, n_);

    for (int k=0;k<K_;++k) {
      R = mul(mul(Ass[k], R), Ass[k].T()) + Vss[k];
    }

    std::vector< MX > Assr(K_);
    std::reverse_copy(Ass.begin(), Ass.end(), Assr.begin());

    MX Ap = mul(Assr);

    // Create an dlesolver instance
    solver_ = DleSolver(getOption(solvername()), dleStruct("a", Ap.sparsity(), "v", R.sparsity()));
    solver_.setOption(getOption(optionsname()));

    // Initialize the NLP solver
    solver_.init();

    std::vector<MX> Pr = solver_.call(dpleIn("a", Ap, "v", R));

    std::vector<MX> Ps(K_);
    Ps[0] = Pr[0];

    for (int k=0;k<K_-1;++k) {
      Ps[k+1] = mul(mul(Ass[k], Ps[k]), Ass[k].T()) + Vss[k];
    }

    f_ = MXFunction(dpleIn("a", As, "v", Vs), dpleOut("p", horzcat(Ps)));
    f_.init();

    Wrapper::checkDimensions();

  }
Exemplo n.º 3
0
RealtimeAPCSCP::RealtimeAPCSCP(Model* model) :
	RealtimeAPCSCP::Solver(model) {
	this->nlp = MXFunction("nlp", nlpIn("x", model->getV()), nlpOut("f",
			model->getf(0), "g", G));
	this->dG_f = this->nlp.jacobian("x", "g");
	this->dG_fast = this->nlp.derReverse(1);
	this->choiceH = 0;
	this->exactA = true;
	this->n_updateA = 1;
	this->n_updateH = 1;
	this->assumedData = std::vector<Matrix<double> >(this->model->getN_F() + 1);
	this->firstIteration = true;
	this->name = "APCSCP";
	this->hess_gi = std::vector<Function>(G.size());
	for (int i = 0; i < G.size(); i++) {
		hess_gi[i] = MXFunction("constraint_i", nlpIn("x", model->getV()),
				nlpOut("g", (MX) G[i])).hessian("x", "g");
	}
	hess_fi = nlp.hessian("x", "f");
}
Exemplo n.º 4
0
  Function SwitchInternal
  ::getDerReverse(const std::string& name, int nadj, Dict& opts) {
    // Derivative of each case
    vector<Function> der(f_.size());
    for (int k=0; k<f_.size(); ++k) {
      if (!f_[k].isNull()) der[k] = f_[k].derReverse(nadj);
    }

    // Default case
    Function der_def;
    if (!f_def_.isNull()) der_def = f_def_.derReverse(nadj);

    // New Switch for derivatives
    stringstream ss;
    ss << "adj" << nadj << "_" << name_;
    Switch sw(ss.str(), der, der_def);

    // Construct wrapper inputs and arguments for calling sw
    vector<MX> arg = symbolicInput();
    vector<MX> res = symbolicOutput();
    vector<vector<MX> > seed = symbolicAdjSeed(nadj, res);
    vector<MX> w_in = arg;
    w_in.insert(w_in.end(), res.begin(), res.end());
    // Arguments for calling sw being constructed
    vector<MX> v;
    v.push_back(arg.at(0)); // index
    for (int d=0; d<nadj; ++d) {
      // Add to wrapper input
      w_in.insert(w_in.end(), seed[d].begin(), seed[d].end());
      // Add to sw argument vector
      v.insert(v.end(), arg.begin()+1, arg.end());
      v.insert(v.end(), res.begin(), res.end());
      v.insert(v.end(), seed[d].begin(), seed[d].end());
    }

    // Construct wrapper outputs
    casadi_assert(v.size()==sw.nIn());
    v = sw(v);
    vector<MX> w_out;
    MX ind_sens = MX(1, 1); // no dependency on index
    vector<MX>::const_iterator v_it = v.begin(), v_it_next;
    for (int d=0; d<nadj; ++d) {
      w_out.push_back(ind_sens);
      v_it_next = v_it + (nIn()-1);
      w_out.insert(w_out.end(), v_it, v_it_next);
      v_it = v_it_next;
    }

    // Create wrapper
    return MXFunction(name, w_in, w_out, opts);
  }
  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();
  }
Exemplo n.º 6
0
FX ParallelizerInternal::getDerivative(int nfwd, int nadj){
  // Generate derivative expressions
  vector<FX> der_funcs(funcs_.size());
  for(int i=0; i<funcs_.size(); ++i){
    der_funcs[i] = funcs_[i].derivative(nfwd,nadj);
  }
  
  // Create a new parallelizer for the derivatives
  Parallelizer par(der_funcs);
  
  // Set options and initialize
  par.setOption(dictionary());
  par.init();
  
  // Create a function call to the parallelizer
  vector<MX> par_arg = par.symbolicInput();
  vector<MX> par_res = par.call(par_arg);
  
  // Get the offsets: copy to allow being used as a counter
  std::vector<int> par_inind = par->inind_;
  std::vector<int> par_outind = par->outind_;
  
  // Arguments and results of the return function
  vector<MX> ret_arg, ret_res;
  ret_arg.reserve(par_arg.size());
  ret_res.reserve(par_res.size());
  
  // Loop over all nondifferentiated inputs/outputs and forward seeds/sensitivities
  for(int dir=-1; dir<nfwd; ++dir){
    for(int i=0; i<funcs_.size(); ++i){
      for(int j=inind_[i]; j<inind_[i+1]; ++j) ret_arg.push_back(par_arg[par_inind[i]++]);
      for(int j=outind_[i]; j<outind_[i+1]; ++j) ret_res.push_back(par_res[par_outind[i]++]);
    }
  }
  
  // Loop over adjoint seeds/sensitivities
  for(int dir=0; dir<nadj; ++dir){
    for(int i=0; i<funcs_.size(); ++i){
      for(int j=outind_[i]; j<outind_[i+1]; ++j) ret_arg.push_back(par_arg[par_inind[i]++]);
      for(int j=inind_[i]; j<inind_[i+1]; ++j) ret_res.push_back(par_res[par_outind[i]++]);
    }
  }
  
  // Assemble the return function
  return MXFunction(ret_arg,ret_res);
}
  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();

  }
Exemplo n.º 8
0
  Function SwitchInternal
  ::getDerForward(const std::string& name, int nfwd, Dict& opts) {
    // Derivative of each case
    vector<Function> der(f_.size());
    for (int k=0; k<f_.size(); ++k) {
      if (!f_[k].isNull()) der[k] = f_[k].derForward(nfwd);
    }

    // Default case
    Function der_def;
    if (!f_def_.isNull()) der_def = f_def_.derForward(nfwd);

    // New Switch for derivatives
    stringstream ss;
    ss << "fwd" << nfwd << "_" << name_;
    Switch sw(ss.str(), der, der_def);

    // Construct wrapper inputs and arguments for calling sw
    vector<MX> arg = symbolicInput();
    vector<MX> res = symbolicOutput();
    vector<vector<MX> > seed = symbolicFwdSeed(nfwd, arg);
    // Wrapper input being constructed
    vector<MX> w_in = arg;
    w_in.insert(w_in.end(), res.begin(), res.end());
    // Arguments for calling sw being constructed
    vector<MX> v;
    v.insert(v.end(), arg.begin(), arg.end());
    v.insert(v.end(), res.begin(), res.end());
    for (int d=0; d<nfwd; ++d) {
      // ignore seed for ind
      seed[d][0] = MX::sym(seed[d][0].getName(), Sparsity::scalar(false));
      // Add to wrapper input
      w_in.insert(w_in.end(), seed[d].begin(), seed[d].end());
      // Add to sw argument vector
      v.insert(v.end(), seed[d].begin()+1, seed[d].end());
    }

    // Create wrapper
    casadi_assert(v.size()==sw.nIn());
    return MXFunction(name, w_in, sw(v), opts);
  }
  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 Vss = (Vs+Vs.T())/2;

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

    MX Pf = solve(A_total, vec(Vss), getOption("linear_solver"));

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

    f_ = MXFunction(dleIn("a", As, "v", Vs),
      dleOut("p", MX(P(output().sparsity()))));

    f_.init();

    casadi_assert(getNumOutputs()==f_.getNumOutputs());
    for (int i=0;i<getNumInputs();++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<getNumOutputs();++i) {
      casadi_assert_message(output(i).sparsity()==f_.output(i).sparsity(),
        "Sparsity mismatch for output " << i << ":" <<
        output(i).dimString() << " <-> " << f_.output(i).dimString() << ".");
    }
  }
Exemplo n.º 10
0
MXFunction vec (const FX &a_) {
  FX a = a_;

  // Pass null if input is null
  if (a.isNull()) return MXFunction();
  
  // Get the MX inputs, only used for shape
  const std::vector<MX> &symbolicInputMX = a.symbolicInput();
  // Have a vector with MX that have the shape of vec(symbolicInputMX )
  std::vector<MX> symbolicInputMX_vec(a.getNumInputs());
  // Make vector valued MX's out of them
  std::vector<MX> symbolicInputMX_vec_reshape(a.getNumInputs());

  // Apply the vec-transformation to the inputs
  for (int i=0;i<symbolicInputMX.size();++i) {
    std::stringstream s;
    s << "X_flat_" << i;
    symbolicInputMX_vec[i] = MX(s.str(),vec(symbolicInputMX[i].sparsity()));
    symbolicInputMX_vec_reshape[i] = trans(reshape(symbolicInputMX_vec[i],trans(symbolicInputMX[i].sparsity())));
  }
  
  // Call the original function with the vecced inputs
  std::vector<MX> symbolicOutputMX = a.call(symbolicInputMX_vec_reshape);
  
  // Apply the vec-transformation to the outputs
  for (int i=0;i<symbolicOutputMX.size();++i)
    symbolicOutputMX[i] = vec(symbolicOutputMX[i]);
    
  // Make a new function with the vecced input/outputs
  MXFunction ret(symbolicInputMX_vec,symbolicOutputMX);
  
  // Initialize it if a was
  if (a.isInit()) ret.init();
  return ret;

}  
Exemplo n.º 11
0
  Function implicitRK(Function& f, const std::string& impl, const Dictionary& impl_options,
                      const MX& tf, int order, const std::string& scheme, int ne) {
    casadi_assert_message(ne>=1, "Parameter ne (number of elements must be at least 1), "
                          "but got " << ne << ".");
    casadi_assert_message(order==4, "Only RK order 4 is supported now.");
    casadi_assert_message(f.getNumInputs()==DAE_NUM_IN && f.getNumOutputs()==DAE_NUM_OUT,
                          "Supplied function must adhere to dae scheme.");
    casadi_assert_message(f.output(DAE_QUAD).isEmpty(),
                          "Supplied function cannot have quadrature states.");

    // Obtain collocation points
    std::vector<double> tau_root = collocationPoints(order, "legendre");

    // Retrieve collocation interpolating matrices
    std::vector < std::vector <double> > C;
    std::vector < double > D;
    collocationInterpolators(tau_root, C, D);

    // Retrieve problem dimensions
    int nx = f.input(DAE_X).size();
    int nz = f.input(DAE_Z).size();
    int np = f.input(DAE_P).size();

    //Variables for one finite element
    MX X = MX::sym("X", nx);
    MX P = MX::sym("P", np);
    MX V = MX::sym("V", order*(nx+nz)); // Unknowns

    MX X0 = X;

    // Components of the unknowns that correspond to states at collocation points
    std::vector<MX> Xc;Xc.reserve(order);
    Xc.push_back(X0);

    // Components of the unknowns that correspond to algebraic states at collocation points
    std::vector<MX> Zc;Zc.reserve(order);

    // Splitting the unknowns
    std::vector<int> splitPositions = range(0, order*nx, nx);
    if (nz>0) {
      std::vector<int> Zc_pos = range(order*nx, order*nx+(order+1)*nz, nz);
      splitPositions.insert(splitPositions.end(), Zc_pos.begin(), Zc_pos.end());
    } else {
      splitPositions.push_back(order*nx);
    }
    std::vector<MX> Vs = vertsplit(V, splitPositions);

    // Extracting unknowns from Z
    for (int i=0;i<order;++i) {
      Xc.push_back(X0+Vs[i]);
    }
    if (nz>0) {
      for (int i=0;i<order;++i) {
        Zc.push_back(Vs[order+i]);
      }
    }

    // Get the collocation Equations (that define V)
    std::vector<MX> V_eq;

    // Local start time
    MX t0_l=MX::sym("t0");
    MX h = MX::sym("h");

    for (int j=1;j<order+1;++j) {
      // Expression for the state derivative at the collocation point
      MX xp_j = 0;
      for (int r=0;r<order+1;++r) {
        xp_j+= C[j][r]*Xc[r];
      }
      // Append collocation equations & algebraic constraints
      std::vector<MX> f_out;
      MX t_l = t0_l+tau_root[j]*h;
      if (nz>0) {
        f_out = f.call(daeIn("t", t_l, "x", Xc[j], "p", P, "z", Zc[j-1]));
      } else {
        f_out = f.call(daeIn("t", t_l, "x", Xc[j], "p", P));
      }
      V_eq.push_back(h*f_out[DAE_ODE]-xp_j);
      V_eq.push_back(f_out[DAE_ALG]);

    }

    // Root-finding function, implicitly defines V as a function of X0 and P
    std::vector<MX> vfcn_inputs;
    vfcn_inputs.push_back(V);
    vfcn_inputs.push_back(X);
    vfcn_inputs.push_back(P);
    vfcn_inputs.push_back(t0_l);
    vfcn_inputs.push_back(h);

    Function vfcn = MXFunction(vfcn_inputs, vertcat(V_eq));
    vfcn.init();

    try {
      // Attempt to convert to SXFunction to decrease overhead
      vfcn = SXFunction(vfcn);
      vfcn.init();
    } catch(CasadiException & e) {
      //
    }

    // Create a implicit function instance to solve the system of equations
    ImplicitFunction ifcn(impl, vfcn, Function(), LinearSolver());
    ifcn.setOption(impl_options);
    ifcn.init();

    // Get an expression for the state at the end of the finite element
    std::vector<MX> ifcn_call_in(5);
    ifcn_call_in[0] = MX::zeros(V.sparsity());
    std::copy(vfcn_inputs.begin()+1, vfcn_inputs.end(), ifcn_call_in.begin()+1);
    std::vector<MX> ifcn_call_out = ifcn.call(ifcn_call_in, true);
    Vs = vertsplit(ifcn_call_out[0], splitPositions);

    MX XF = 0;
    for (int i=0;i<order+1;++i) {
      XF += D[i]*(i==0? X : X + Vs[i-1]);
    }


    // Get the discrete time dynamics
    ifcn_call_in.erase(ifcn_call_in.begin());
    MXFunction F = MXFunction(ifcn_call_in, XF);
    F.init();

    // Loop over all finite elements
    MX h_ = tf/ne;
    MX t0_ = 0;

    for (int i=0;i<ne;++i) {
      std::vector<MX> F_in;
      F_in.push_back(X);
      F_in.push_back(P);
      F_in.push_back(t0_);
      F_in.push_back(h_);
      t0_+= h_;
      std::vector<MX> F_out = F.call(F_in);
      X = F_out[0];
    }

    // Create a ruturn function with Integrator signature
    MXFunction ret = MXFunction(integratorIn("x0", X0, "p", P), integratorOut("xf", X));
    ret.init();

    return ret;

  }
Exemplo n.º 12
0
void NLPSolverInternal::init(){
  // Read options
  verbose_ = getOption("verbose");
  gauss_newton_ = getOption("gauss_newton");
  
  // Initialize the functions
  casadi_assert_message(!F_.isNull(),"No objective function");
  if(!F_.isInit()){
    F_.init();
    log("Objective function initialized");
  }
  if(!G_.isNull() && !G_.isInit()){
    G_.init();
    log("Constraint function initialized");
  }

  // Get dimensions
  n_ = F_.input(0).numel();
  m_ = G_.isNull() ? 0 : G_.output(0).numel();

  parametric_ = getOption("parametric");
  
  if (parametric_) {
    casadi_assert_message(F_.getNumInputs()==2, "Wrong number of input arguments to F for parametric NLP. Must be 2, but got " << F_.getNumInputs());
  } else {
    casadi_assert_message(F_.getNumInputs()==1, "Wrong number of input arguments to F for non-parametric NLP. Must be 1, but got " << F_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option.");
  }

  // Basic sanity checks
  casadi_assert_message(F_.getNumInputs()==1 || F_.getNumInputs()==2, "Wrong number of input arguments to F. Must be 1 or 2");
  
  if (F_.getNumInputs()==2) parametric_=true;
  casadi_assert_message(getOption("ignore_check_vec") || gauss_newton_ || F_.input().size2()==1,
     "To avoid confusion, the input argument to F must be vector. You supplied " << F_.input().dimString() << endl <<
     " We suggest you make the following changes:" << endl <<
     "   -  F is an SXFunction:  SXFunction([X],[rhs]) -> SXFunction([vec(X)],[rhs])" << endl <<
     "             or            F -                   ->  F = vec(F) " << 
     "   -  F is an MXFunction:  MXFunction([X],[rhs]) -> " <<  endl <<
     "                                     X_vec = MX(\"X\",vec(X.sparsity())) " << endl <<
     "                                     F_vec = MXFunction([X_flat],[F.call([X_flat.reshape(X.sparsity())])[0]]) " << endl <<
     "             or            F -                   ->  F = vec(F) " << 
     " You may ignore this warning by setting the 'ignore_check_vec' option to true." << endl
  );
  
  casadi_assert_message(F_.getNumOutputs()>=1, "Wrong number of output arguments to F");
  casadi_assert_message(gauss_newton_  || F_.output().scalar(), "Output argument of F not scalar.");
  casadi_assert_message(F_.output().dense(), "Output argument of F not dense.");
  casadi_assert_message(F_.input().dense(), "Input argument of F must be dense. You supplied " << F_.input().dimString());
  
  if(!G_.isNull()) {
    if (parametric_) {
      casadi_assert_message(G_.getNumInputs()==2, "Wrong number of input arguments to G for parametric NLP. Must be 2, but got " << G_.getNumInputs());
    } else {
      casadi_assert_message(G_.getNumInputs()==1, "Wrong number of input arguments to G for non-parametric NLP. Must be 1, but got " << G_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option.");
    }
    casadi_assert_message(G_.getNumOutputs()>=1, "Wrong number of output arguments to G");
    casadi_assert_message(G_.input().numel()==n_, "Inconsistent dimensions");
    casadi_assert_message(G_.input().sparsity()==F_.input().sparsity(), "F and G input dimension must match. F " << F_.input().dimString() << ". G " << G_.input().dimString());
  }
  
  // Find out if we are to expand the objective function in terms of scalar operations
  bool expand_f = getOption("expand_f");
  if(expand_f){
    log("Expanding objective function");
    
    // Cast to MXFunction
    MXFunction F_mx = shared_cast<MXFunction>(F_);
    if(F_mx.isNull()){
      casadi_warning("Cannot expand objective function as it is not an MXFunction");
    } else {
      // Take use the input scheme of G if possible (it might be an SXFunction)
      vector<SXMatrix> inputv;
      if(!G_.isNull() && F_.getNumInputs()==G_.getNumInputs()){
        inputv = G_.symbolicInputSX();
      } else {
        inputv = F_.symbolicInputSX();
      }
      
      // Try to expand the MXFunction
      F_ = F_mx.expand(inputv);
      F_.setOption("number_of_fwd_dir",F_mx.getOption("number_of_fwd_dir"));
      F_.setOption("number_of_adj_dir",F_mx.getOption("number_of_adj_dir"));
      F_.init();
    }
  }
  
  
  // Find out if we are to expand the constraint function in terms of scalar operations
  bool expand_g = getOption("expand_g");
  if(expand_g){
    log("Expanding constraint function");
    
    // Cast to MXFunction
    MXFunction G_mx = shared_cast<MXFunction>(G_);
    if(G_mx.isNull()){
      casadi_warning("Cannot expand constraint function as it is not an MXFunction");
    } else {
      // Take use the input scheme of F if possible (it might be an SXFunction)
      vector<SXMatrix> inputv;
      if(F_.getNumInputs()==G_.getNumInputs()){
        inputv = F_.symbolicInputSX();
      } else {
        inputv = G_.symbolicInputSX();
      }
      
      // Try to expand the MXFunction
      G_ = G_mx.expand(inputv);
      G_.setOption("number_of_fwd_dir",G_mx.getOption("number_of_fwd_dir"));
      G_.setOption("number_of_adj_dir",G_mx.getOption("number_of_adj_dir"));
      G_.init();
    }
  }
  
  // Find out if we are to expand the constraint function in terms of scalar operations
  bool generate_hessian = getOption("generate_hessian");
  if(generate_hessian && H_.isNull()){
    casadi_assert_message(!gauss_newton_,"Automatic generation of Gauss-Newton Hessian not yet supported");
    log("generating hessian");
    
    // Simple if unconstrained
    if(G_.isNull()){
      // Create Hessian of the objective function
      FX HF = F_.hessian();
      HF.init();
      
      // Symbolic inputs of HF
      vector<MX> HF_in = F_.symbolicInput();
      
      // Lagrange multipliers
      MX lam("lam",0);
      
      // Objective function scaling
      MX sigma("sigma");
      
      // Inputs of the Hessian function
      vector<MX> H_in = HF_in;
      H_in.insert(H_in.begin()+1, lam);
      H_in.insert(H_in.begin()+2, sigma);

      // Get an expression for the Hessian of F
      MX hf = HF.call(HF_in).at(0);
      
      // Create the scaled Hessian function
      H_ = MXFunction(H_in, sigma*hf);
      log("Unconstrained Hessian function generated");
      
    } else { // G_.isNull()
      
      // Check if the functions are SXFunctions
      SXFunction F_sx = shared_cast<SXFunction>(F_);
      SXFunction G_sx = shared_cast<SXFunction>(G_);
      
      // Efficient if both functions are SXFunction
      if(!F_sx.isNull() && !G_sx.isNull()){
        // Expression for f and g
        SXMatrix f = F_sx.outputSX();
        SXMatrix g = G_sx.outputSX();
        
        // Numeric hessian
        bool f_num_hess = F_sx.getOption("numeric_hessian");
        bool g_num_hess = G_sx.getOption("numeric_hessian");
        
        // Number of derivative directions
        int f_num_fwd = F_sx.getOption("number_of_fwd_dir");
        int g_num_fwd = G_sx.getOption("number_of_fwd_dir");
        int f_num_adj = F_sx.getOption("number_of_adj_dir");
        int g_num_adj = G_sx.getOption("number_of_adj_dir");
        
        // Substitute symbolic variables in f if different input variables from g
        if(!isEqual(F_sx.inputSX(),G_sx.inputSX())){
          f = substitute(f,F_sx.inputSX(),G_sx.inputSX());
        }
        
        // Lagrange multipliers
        SXMatrix lam = ssym("lambda",g.size1());

        // Objective function scaling
        SXMatrix sigma = ssym("sigma");        
        
        // Lagrangian function
        vector<SXMatrix> lfcn_in(parametric_? 4: 3);
        lfcn_in[0] = G_sx.inputSX();
        lfcn_in[1] = lam;
        lfcn_in[2] = sigma;
        if (parametric_) lfcn_in[3] = G_sx.inputSX(1);
        SXFunction lfcn(lfcn_in, sigma*f + inner_prod(lam,g));
        lfcn.setOption("verbose",getOption("verbose"));
        lfcn.setOption("numeric_hessian",f_num_hess || g_num_hess);
        lfcn.setOption("number_of_fwd_dir",std::min(f_num_fwd,g_num_fwd));
        lfcn.setOption("number_of_adj_dir",std::min(f_num_adj,g_num_adj));
        lfcn.init();
        
        // Hessian of the Lagrangian
        H_ = static_cast<FX&>(lfcn).hessian();
        H_.setOption("verbose",getOption("verbose"));
        log("SX Hessian function generated");
        
      } else { // !F_sx.isNull() && !G_sx.isNull()
        // Check if the functions are SXFunctions
        MXFunction F_mx = shared_cast<MXFunction>(F_);
        MXFunction G_mx = shared_cast<MXFunction>(G_);
        
        // If they are, check if the arguments are the same
        if(!F_mx.isNull() && !G_mx.isNull() && isEqual(F_mx.inputMX(),G_mx.inputMX())){
          casadi_warning("Exact Hessian calculation for MX is still experimental");
          
          // Expression for f and g
          MX f = F_mx.outputMX();
          MX g = G_mx.outputMX();
          
          // Lagrange multipliers
          MX lam("lam",g.size1());
      
          // Objective function scaling
          MX sigma("sigma");

          // Inputs of the Lagrangian function
          vector<MX> lfcn_in(parametric_? 4:3);
          lfcn_in[0] = G_mx.inputMX();
          lfcn_in[1] = lam;
          lfcn_in[2] = sigma;
          if (parametric_) lfcn_in[3] = G_mx.inputMX(1);

          // Lagrangian function
          MXFunction lfcn(lfcn_in,sigma*f+ inner_prod(lam,g));
          lfcn.init();
	  log("SX Lagrangian function generated");
          
/*          cout << "countNodes(lfcn.outputMX()) = " << countNodes(lfcn.outputMX()) << endl;*/
      
          bool adjoint_mode = true;
          if(adjoint_mode){
          
            // Gradient of the lagrangian
            MX gL = lfcn.grad();
            log("MX Lagrangian gradient generated");

            MXFunction glfcn(lfcn_in,gL);
            glfcn.init();
            log("MX Lagrangian gradient function initialized");
//           cout << "countNodes(glfcn.outputMX()) = " << countNodes(glfcn.outputMX()) << endl;

            // Get Hessian sparsity
            CRSSparsity H_sp = glfcn.jacSparsity();
            log("MX Lagrangian Hessian sparsity determined");
            
            // Uni-directional coloring (note, the hessian is symmetric)
            CRSSparsity coloring = H_sp.unidirectionalColoring(H_sp);
            log("MX Lagrangian Hessian coloring determined");

            // Number of colors needed is the number of rows
            int nfwd_glfcn = coloring.size1();
            log("MX Lagrangian gradient function number of sensitivity directions determined");

            glfcn.setOption("number_of_fwd_dir",nfwd_glfcn);
            glfcn.updateNumSens();
            log("MX Lagrangian gradient function number of sensitivity directions updated");
            
            // Hessian of the Lagrangian
            H_ = glfcn.jacobian();
          } else {

            // Hessian of the Lagrangian
            H_ = lfcn.hessian();
            
          }
          log("MX Lagrangian Hessian function generated");
          
        } else {
          casadi_assert_message(0, "Automatic calculation of exact Hessian currently only for F and G both SXFunction or MXFunction ");
        }
      } // !F_sx.isNull() && !G_sx.isNull()
    } // G_.isNull()
  } // generate_hessian && H_.isNull()
  if(!H_.isNull() && !H_.isInit()) {
    H_.init();
    log("Hessian function initialized");
  }

  // Create a Jacobian if it does not already exists
  bool generate_jacobian = getOption("generate_jacobian");
  if(generate_jacobian && !G_.isNull() && J_.isNull()){
    log("Generating Jacobian");
    J_ = G_.jacobian();
    
    // Use live variables if SXFunction
    if(!shared_cast<SXFunction>(J_).isNull()){
      J_.setOption("live_variables",true);
    }
    log("Jacobian function generated");
  }
    
  if(!J_.isNull() && !J_.isInit()){
    J_.init();
    log("Jacobian function initialized");
  }

  
  if(!H_.isNull()) {
    if (parametric_) {
      casadi_assert_message(H_.getNumInputs()>=2, "Wrong number of input arguments to H for parametric NLP. Must be at least 2, but got " << G_.getNumInputs());
    } else {
      casadi_assert_message(H_.getNumInputs()>=1, "Wrong number of input arguments to H for non-parametric NLP. Must be at least 1, but got " << G_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option.");
    }
    casadi_assert_message(H_.getNumOutputs()>=1, "Wrong number of output arguments to H");
    casadi_assert_message(H_.input(0).numel()==n_,"Inconsistent dimensions");
    casadi_assert_message(H_.output().size1()==n_,"Inconsistent dimensions");
    casadi_assert_message(H_.output().size2()==n_,"Inconsistent dimensions");
  }

  if(!J_.isNull()){
    if (parametric_) {
      casadi_assert_message(J_.getNumInputs()==2, "Wrong number of input arguments to J for parametric NLP. Must be at least 2, but got " << G_.getNumInputs());
    } else {
      casadi_assert_message(J_.getNumInputs()==1, "Wrong number of input arguments to J for non-parametric NLP. Must be at least 1, but got " << G_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option.");
    }
    casadi_assert_message(J_.getNumOutputs()>=1, "Wrong number of output arguments to J");
    casadi_assert_message(J_.input().numel()==n_,"Inconsistent dimensions");
    casadi_assert_message(J_.output().size2()==n_,"Inconsistent dimensions");
  }

  if (parametric_) {
    sp_p = F_->input(1).sparsity();
    
    if (!G_.isNull()) casadi_assert_message(sp_p == G_->input(G_->getNumInputs()-1).sparsity(),"Parametric NLP has inconsistent parameter dimensions. F has got " << sp_p.dimString() << " as dimensions, while G has got " << G_->input(G_->getNumInputs()-1).dimString());
    if (!H_.isNull()) casadi_assert_message(sp_p == H_->input(H_->getNumInputs()-1).sparsity(),"Parametric NLP has inconsistent parameter dimensions. F has got " << sp_p.dimString() << " as dimensions, while H has got " << H_->input(H_->getNumInputs()-1).dimString());
    if (!J_.isNull()) casadi_assert_message(sp_p == J_->input(J_->getNumInputs()-1).sparsity(),"Parametric NLP has inconsistent parameter dimensions. F has got " << sp_p.dimString() << " as dimensions, while J has got " << J_->input(J_->getNumInputs()-1).dimString());
  }
  
  // Infinity
  double inf = numeric_limits<double>::infinity();
  
  // Allocate space for inputs
  input_.resize(NLP_NUM_IN - (parametric_? 0 : 1));
  input(NLP_X_INIT)      = DMatrix(n_,1,0);
  input(NLP_LBX)         = DMatrix(n_,1,-inf);
  input(NLP_UBX)         = DMatrix(n_,1, inf);
  input(NLP_LBG)         = DMatrix(m_,1,-inf);
  input(NLP_UBG)         = DMatrix(m_,1, inf);
  input(NLP_LAMBDA_INIT) = DMatrix(m_,1,0);
  if (parametric_) input(NLP_P) = DMatrix(sp_p,0);
  
  // Allocate space for outputs
  output_.resize(NLP_NUM_OUT);
  output(NLP_X_OPT)      = DMatrix(n_,1,0);
  output(NLP_COST)       = DMatrix(1,1,0);
  output(NLP_LAMBDA_X)   = DMatrix(n_,1,0);
  output(NLP_LAMBDA_G)   = DMatrix(m_,1,0);
  output(NLP_G)          = DMatrix(m_,1,0);
  
  if (hasSetOption("iteration_callback")) {
   callback_ = getOption("iteration_callback");
   if (!callback_.isNull()) {
     if (!callback_.isInit()) callback_.init();
     casadi_assert_message(callback_.getNumOutputs()==1, "Callback function should have one output, a scalar that indicates wether to break. 0 = continue");
     casadi_assert_message(callback_.output(0).size()==1, "Callback function should have one output, a scalar that indicates wether to break. 0 = continue");
     casadi_assert_message(callback_.getNumInputs()==NLP_NUM_OUT, "Callback function should have the output scheme of NLPSolver as input scheme. i.e. " <<NLP_NUM_OUT << " inputs instead of the " << callback_.getNumInputs() << " you provided." );
     for (int i=0;i<NLP_NUM_OUT;i++) {
       casadi_assert_message(callback_.input(i).sparsity()==output(i).sparsity(),
         "Callback function should have the output scheme of NLPSolver as input scheme. " << 
         "Input #" << i << " (" << getSchemeEntryEnumName(SCHEME_NLPOutput,i) <<  " aka '" << getSchemeEntryName(SCHEME_NLPOutput,i) << "') was found to be " << callback_.input(i).dimString() << " instead of expected " << output(i).dimString() << "."
       );
       callback_.input(i).setAll(0);
     }
   }
  }
  
  callback_step_ = getOption("iteration_callback_step");

  // Call the initialization method of the base class
  FXInternal::init();
}
Exemplo n.º 13
0
void RealtimeAPCSCP::solve() {
	//Initialisation (calculate the exact solution for inital point)
	int t_act = 0;
	std::cout << "Time: " + to_string(t_act) << std::endl;
	startIt = std::chrono::system_clock::now();
	startSolver = std::chrono::system_clock::now();

	//Solve the first problem exactly
	std::map<std::string, Matrix<double> > arg = make_map("lbx",
			model->getVMIN(), "ubx", model->getVMAX(), "lbg", this->G_bound,
			"ubg", this->G_bound, "x0", model->getVINIT());
	NlpSolver nlpSolver = NlpSolver("solver", "ipopt", nlp, opts);
	nlpSolver.setOption("warn_initial_bounds", true);
	nlpSolver.setOption("eval_errors_fatal", true);
	std::map<string, Matrix<double>> result_tact = nlpSolver(arg);
	endSolver = std::chrono::system_clock::now();
	// Store data
	assumedData[t_act] = result_tact["x"];

	//Setup new iteration
	model->storeAndShiftValues(result_tact, t_act);
	storeStatsIpopt(&nlpSolver);

	//Define values
	this->x_act = result_tact["x"];
	this->y_act = result_tact["lam_g"];

	evaluateOriginalF(t_act,x_act);
	updateA_act(t_act);
	updateH_act(t_act);
	updateM_act();
	m_act = mul(transpose(Dg_act - A_act), y_act);
	firstIteration = false;
	opts["warm_start_init_point"] = "yes";
	endIt = std::chrono::system_clock::now();
	printTimingData(t_act);

	//Iteration
	for (t_act = 1; t_act < model->getN_F(); t_act++) {
		std::cout << "Time: " + to_string(t_act) << std::endl;
		startIt = std::chrono::system_clock::now();

		startMS = std::chrono::system_clock::now();
		G_sub = mul(A_act, model->getV() - x_act) + model->doMultipleShooting(x_act);
		endMS = std::chrono::system_clock::now();
		f_sub = model->getf(t_act) + mul(transpose(m_act), model->getV()
				- x_act) + 0.5 * quad_form(model->getV() - x_act, H_act);

		this->nlp_sub = MXFunction("nlp", nlpIn("x", model->getV()), nlpOut(
				"f", f_sub, "g", G_sub));
		this->nlp = MXFunction("nlp", nlpIn("x", model->getV()), nlpOut("f",
				model->getf(t_act), "g", G));

		// Step2 solve convex subproblem
		startSolver = std::chrono::system_clock::now();
		result_tact = solveConvexSubproblem();
		endSolver = std::chrono::system_clock::now();

		// Step3 update matrices and retrieve new measurement (step 1)
		x_act = result_tact["x"];
		y_act = result_tact["lam_g"];
		model->storeAndShiftValues(result_tact, t_act);

		// update
		this->x_act = result_tact["x"];
		this->y_act = result_tact["lam_g"];

		evaluateOriginalF(t_act, x_act);
		updateA_act(t_act);
		updateH_act(t_act);
		updateM_act();

		endIt = std::chrono::system_clock::now();
		printTimingData(t_act);
	}
}
Exemplo n.º 14
0
void GslInternal::init(){
  // Init ODE rhs function and quadrature functions
  f_.init();
  casadi_assert(f_.getNumInputs()==DAE_NUM_IN);
  casadi_assert(f_.getNumOutputs()==DAE_NUM_OUT);
  if(!q_.isNull()){
    q_.init();
    casadi_assert(q_.getNumInputs()==DAE_NUM_IN);
    casadi_assert(q_.getNumOutputs()==DAE_NUM_OUT);
  }

  // Number of states
  int nx = f_.output(INTEGRATOR_XF).numel();

  // Add quadratures, if any
  if(!q_.isNull()) nx += q_.output().numel();

  // Number of parameters
  int np = f_.input(DAE_P).numel();

  setDimensions(nx,np);

  // If time was not specified, initialise it.
  if (f_.input(DAE_T).numel()==0) {
    std::vector<MX> in1(DAE_NUM_IN);
    in1[DAE_T] = MX("T");
    in1[DAE_Y] = MX("Y",f_.input(DAE_Y).size1(),f_.input(DAE_Y).size2());
    in1[DAE_YDOT] = MX("YDOT",f_.input(DAE_YDOT).size1(),f_.input(DAE_YDOT).size2());
    in1[DAE_P] = MX("P",f_.input(DAE_P).size1(),f_.input(DAE_P).size2());
    std::vector<MX> in2(in1);
    in2[DAE_T] = MX();
    f_ = MXFunction(in1,f_.call(in2));
    f_.init();
  }
  
  // We only allow for 0-D time
  casadi_assert_message(f_.input(DAE_T).numel()==1, "IntegratorInternal: time must be zero-dimensional, not (" <<  f_.input(DAE_T).size1() << 'x' << f_.input(DAE_T).size2() << ")");
  
  // ODE right hand side must be a dense matrix
  casadi_assert_message(f_.output(DAE_RES).dense(),"ODE right hand side must be dense: reformulate the problem");
  
  // States and RHS should match 
  casadi_assert_message(f_.output(DAE_RES).size()==f_.input(DAE_Y).size(),
    "IntegratorInternal: rhs of ODE is (" <<  f_.output(DAE_RES).size1() << 'x' << f_.output(DAE_RES).size2() << ") - " << f_.output(DAE_RES).size() << " non-zeros" << std::endl <<
    "              ODE state matrix is (" <<  f_.input(DAE_Y).size1() << 'x' << f_.input(DAE_Y).size2() << ") - " << f_.input(DAE_Y).size() << " non-zeros" << std::endl <<
    "Mismatch between number of non-zeros"
  );

  IntegratorInternal::init();
  
  jac_f_ = Jacobian(f_,DAE_Y,DAE_RES);
  dt_f_ = Jacobian(f_,DAE_T,DAE_RES);
  
  jac_f_.init();
  dt_f_.init();
  
  // define the type of routine for making steps: 
  type_ptr = gsl_odeiv_step_rkf45;
  // some other possibilities (see GSL manual):          
  //   = gsl_odeiv_step_rk4;
  //   = gsl_odeiv_step_rkck;
  //   = gsl_odeiv_step_rk8pd;
  //   = gsl_odeiv_step_rk4imp;
  //   = gsl_odeiv_step_bsimp;  
  //   = gsl_odeiv_step_gear1;
  //   = gsl_odeiv_step_gear2;
  
  step_ptr = gsl_odeiv_step_alloc (type_ptr, nx);
  control_ptr = gsl_odeiv_control_y_new (abstol_, reltol_);
  evolve_ptr = gsl_odeiv_evolve_alloc (nx);
  
  my_system.function = rhs_wrapper;	// the right-hand-side functions dy[i]/dt 
  my_system.jacobian = jac_wrapper;	// the Jacobian df[i]/dy[j] 
  my_system.dimension = nx;	// number of diffeq's 
  my_system.params = this;	// parameters to pass to rhs and jacobian
  
  is_init = true;
  
}
Exemplo n.º 15
0
  void SQICInternal::init() {
    // Call the init method of the base class
    QpSolverInternal::init();

    if (is_init_) sqicDestroy();

    inf_ = 1.0e+20;

    // Allocate data structures for SQIC
    bl_.resize(n_+nc_+1, 0);
    bu_.resize(n_+nc_+1, 0);
    x_.resize(n_+nc_+1, 0);
    hs_.resize(n_+nc_+1, 0);
    hEtype_.resize(n_+nc_+1, 0);
    pi_.resize(nc_+1, 0);
    rc_.resize(n_+nc_+1, 0);

    locH_ = st_[QP_STRUCT_H].colind();
    indH_ = st_[QP_STRUCT_H].row();

    // Fortran indices are one-based
    for (int i=0;i<indH_.size();++i) indH_[i]+=1;
    for (int i=0;i<locH_.size();++i) locH_[i]+=1;

    // Sparsity of augmented linear constraint matrix
    Sparsity A_ = vertcat(st_[QP_STRUCT_A], Sparsity::dense(1, n_));
    locA_ = A_.colind();
    indA_ = A_.row();

    // Fortran indices are one-based
    for (int i=0;i<indA_.size();++i) indA_[i]+=1;
    for (int i=0;i<locA_.size();++i) locA_[i]+=1;

    // helper functions for augmented linear constraint matrix
    MX a = MX::sym("A", st_[QP_STRUCT_A]);
    MX g = MX::sym("g", n_);
    std::vector<MX> ins;
    ins.push_back(a);
    ins.push_back(g);
    formatA_ = MXFunction(ins, vertcat(a, g.T()));
    formatA_.init();

    // Set objective row of augmented linear constraints
    bu_[n_+nc_] = inf_;
    bl_[n_+nc_] = -inf_;

    is_init_ = true;

    int n = n_;
    int m = nc_+1;

    int nnzA=formatA_.output().size();
    int nnzH=input(QP_SOLVER_H).size();

    std::fill(hEtype_.begin()+n_, hEtype_.end(), 3);

    sqic(&m , &n, &nnzA, &indA_[0], &locA_[0], &formatA_.output().data()[0], &bl_[0], &bu_[0],
         &hEtype_[0], &hs_[0], &x_[0], &pi_[0], &rc_[0], &nnzH, &indH_[0], &locH_[0],
         &input(QP_SOLVER_H).data()[0]);

  }
  void CollocationIntegratorInternal::setupFG() {

    // Interpolation order
    deg_ = getOption("interpolation_order");

    // All collocation time points
    std::vector<long double> tau_root = collocationPointsL(deg_, getOption("collocation_scheme"));

    // Coefficients of the collocation equation
    vector<vector<double> > C(deg_+1, vector<double>(deg_+1, 0));

    // Coefficients of the continuity equation
    vector<double> D(deg_+1, 0);

    // Coefficients of the quadratures
    vector<double> B(deg_+1, 0);

    // For all collocation points
    for (int j=0; j<deg_+1; ++j) {

      // Construct Lagrange polynomials to get the polynomial basis at the collocation point
      Polynomial p = 1;
      for (int r=0; r<deg_+1; ++r) {
        if (r!=j) {
          p *= Polynomial(-tau_root[r], 1)/(tau_root[j]-tau_root[r]);
        }
      }

      // Evaluate the polynomial at the final time to get the
      // coefficients of the continuity equation
      D[j] = zeroIfSmall(p(1.0L));

      // Evaluate the time derivative of the polynomial at all collocation points to
      // get the coefficients of the continuity equation
      Polynomial dp = p.derivative();
      for (int r=0; r<deg_+1; ++r) {
        C[j][r] = zeroIfSmall(dp(tau_root[r]));
      }

      // Integrate polynomial to get the coefficients of the quadratures
      Polynomial ip = p.anti_derivative();
      B[j] = zeroIfSmall(ip(1.0L));
    }

    // Symbolic inputs
    MX x0 = MX::sym("x0", f_.input(DAE_X).sparsity());
    MX p = MX::sym("p", f_.input(DAE_P).sparsity());
    MX t = MX::sym("t", f_.input(DAE_T).sparsity());

    // Implicitly defined variables (z and x)
    MX v = MX::sym("v", deg_*(nx_+nz_));
    vector<int> v_offset(1, 0);
    for (int d=0; d<deg_; ++d) {
      v_offset.push_back(v_offset.back()+nx_);
      v_offset.push_back(v_offset.back()+nz_);
    }
    vector<MX> vv = vertsplit(v, v_offset);
    vector<MX>::const_iterator vv_it = vv.begin();

    // Collocated states
    vector<MX> x(deg_+1), z(deg_+1);
    for (int d=1; d<=deg_; ++d) {
      x[d] = reshape(*vv_it++, this->x0().shape());
      z[d] = reshape(*vv_it++, this->z0().shape());
    }
    casadi_assert(vv_it==vv.end());

    // Collocation time points
    vector<MX> tt(deg_+1);
    for (int d=0; d<=deg_; ++d) {
      tt[d] = t + h_*tau_root[d];
    }

    // Equations that implicitly define v
    vector<MX> eq;

    // Quadratures
    MX qf = MX::zeros(f_.output(DAE_QUAD).sparsity());

    // End state
    MX xf = D[0]*x0;

    // For all collocation points
    for (int j=1; j<deg_+1; ++j) {
      //for (int j=deg_; j>=1; --j) {

      // Evaluate the DAE
      vector<MX> f_arg(DAE_NUM_IN);
      f_arg[DAE_T] = tt[j];
      f_arg[DAE_P] = p;
      f_arg[DAE_X] = x[j];
      f_arg[DAE_Z] = z[j];
      vector<MX> f_res = f_.call(f_arg);

      // Get an expression for the state derivative at the collocation point
      MX xp_j = C[0][j] * x0;
      for (int r=1; r<deg_+1; ++r) {
        xp_j += C[r][j] * x[r];
      }

      // Add collocation equation
      eq.push_back(vec(h_*f_res[DAE_ODE] - xp_j));

      // Add the algebraic conditions
      eq.push_back(vec(f_res[DAE_ALG]));

      // Add contribution to the final state
      xf += D[j]*x[j];

      // Add contribution to quadratures
      qf += (B[j]*h_)*f_res[DAE_QUAD];
    }

    // Form forward discrete time dynamics
    vector<MX> F_in(DAE_NUM_IN);
    F_in[DAE_T] = t;
    F_in[DAE_X] = x0;
    F_in[DAE_P] = p;
    F_in[DAE_Z] = v;
    vector<MX> F_out(DAE_NUM_OUT);
    F_out[DAE_ODE] = xf;
    F_out[DAE_ALG] = vertcat(eq);
    F_out[DAE_QUAD] = qf;
    F_ = MXFunction(F_in, F_out);
    F_.init();

    // Backwards dynamics
    // NOTE: The following is derived so that it will give the exact adjoint
    // sensitivities whenever g is the reverse mode derivative of f.
    if (!g_.isNull()) {

      // Symbolic inputs
      MX rx0 = MX::sym("x0", g_.input(RDAE_RX).sparsity());
      MX rp = MX::sym("p", g_.input(RDAE_RP).sparsity());

      // Implicitly defined variables (rz and rx)
      MX rv = MX::sym("v", deg_*(nrx_+nrz_));
      vector<int> rv_offset(1, 0);
      for (int d=0; d<deg_; ++d) {
        rv_offset.push_back(rv_offset.back()+nrx_);
        rv_offset.push_back(rv_offset.back()+nrz_);
      }
      vector<MX> rvv = vertsplit(rv, rv_offset);
      vector<MX>::const_iterator rvv_it = rvv.begin();

      // Collocated states
      vector<MX> rx(deg_+1), rz(deg_+1);
      for (int d=1; d<=deg_; ++d) {
        rx[d] = reshape(*rvv_it++, this->rx0().shape());
        rz[d] = reshape(*rvv_it++, this->rz0().shape());
      }
      casadi_assert(rvv_it==rvv.end());

      // Equations that implicitly define v
      eq.clear();

      // Quadratures
      MX rqf = MX::zeros(g_.output(RDAE_QUAD).sparsity());

      // End state
      MX rxf = D[0]*rx0;

      // For all collocation points
      for (int j=1; j<deg_+1; ++j) {

        // Evaluate the backward DAE
        vector<MX> g_arg(RDAE_NUM_IN);
        g_arg[RDAE_T] = tt[j];
        g_arg[RDAE_P] = p;
        g_arg[RDAE_X] = x[j];
        g_arg[RDAE_Z] = z[j];
        g_arg[RDAE_RX] = rx[j];
        g_arg[RDAE_RZ] = rz[j];
        g_arg[RDAE_RP] = rp;
        vector<MX> g_res = g_.call(g_arg);

        // Get an expression for the state derivative at the collocation point
        MX rxp_j = -D[j]*rx0;
        for (int r=1; r<deg_+1; ++r) {
          rxp_j += (B[r]*C[j][r]) * rx[r];
        }

        // Add collocation equation
        eq.push_back(vec(h_*B[j]*g_res[RDAE_ODE] - rxp_j));

        // Add the algebraic conditions
        eq.push_back(vec(g_res[RDAE_ALG]));

        // Add contribution to the final state
        rxf += -B[j]*C[0][j]*rx[j];

        // Add contribution to quadratures
        rqf += h_*B[j]*g_res[RDAE_QUAD];
      }

      // Form backward discrete time dynamics
      vector<MX> G_in(RDAE_NUM_IN);
      G_in[RDAE_T] = t;
      G_in[RDAE_X] = x0;
      G_in[RDAE_P] = p;
      G_in[RDAE_Z] = v;
      G_in[RDAE_RX] = rx0;
      G_in[RDAE_RP] = rp;
      G_in[RDAE_RZ] = rv;
      vector<MX> G_out(RDAE_NUM_OUT);
      G_out[RDAE_ODE] = rxf;
      G_out[RDAE_ALG] = vertcat(eq);
      G_out[RDAE_QUAD] = rqf;
      G_ = MXFunction(G_in, G_out);
      G_.init();
    }
  }
Exemplo n.º 17
0
  Function NlpSolver::joinFG(Function F, Function G) {
    if (G.isNull()) {
      // unconstrained
      if (is_a<SXFunction>(F)) {
        SXFunction F_sx = shared_cast<SXFunction>(F);
        vector<SX> nlp_in = F_sx.inputExpr();
        nlp_in.resize(NL_NUM_IN);
        vector<SX> nlp_out(NL_NUM_OUT);
        nlp_out[NL_F] = F_sx.outputExpr(0);
        return SXFunction(nlp_in, nlp_out);
      } else if (is_a<MXFunction>(F)) {
        MXFunction F_mx = shared_cast<MXFunction>(F);
        vector<MX> nlp_in = F_mx.inputExpr();
        nlp_in.resize(NL_NUM_IN);
        vector<MX> nlp_out(NL_NUM_OUT);
        nlp_out[NL_F] = F_mx.outputExpr(0);
        return MXFunction(nlp_in, nlp_out);
      } else {
        vector<MX> F_in = F.symbolicInput();
        vector<MX> nlp_in(NL_NUM_IN);
        nlp_in[NL_X] = F_in.at(0);
        if (F_in.size()>1) nlp_in[NL_P] = F_in.at(1);
        vector<MX> nlp_out(NL_NUM_OUT);
        nlp_out[NL_F] = F(F_in).front();
        return MXFunction(nlp_in, nlp_out);
      }
    } else if (F.isNull()) {
      // feasibility problem
      if (is_a<SXFunction>(G)) {
        SXFunction G_sx = shared_cast<SXFunction>(G);
        vector<SX> nlp_in = G_sx.inputExpr();
        nlp_in.resize(NL_NUM_IN);
        vector<SX> nlp_out(NL_NUM_OUT);
        nlp_out[NL_G] = G_sx.outputExpr(0);
        return SXFunction(nlp_in, nlp_out);
      } else if (is_a<MXFunction>(G)) {
        MXFunction G_mx = shared_cast<MXFunction>(F);
        vector<MX> nlp_in = G_mx.inputExpr();
        nlp_in.resize(NL_NUM_IN);
        vector<MX> nlp_out(NL_NUM_OUT);
        nlp_out[NL_G] = G_mx.outputExpr(0);
        nlp_out.resize(NL_NUM_OUT);
        return MXFunction(nlp_in, nlp_out);
      } else {
        vector<MX> G_in = G.symbolicInput();
        vector<MX> nlp_in(NL_NUM_IN);
        nlp_in[NL_X] = G_in.at(0);
        if (G_in.size()>1) nlp_in[NL_P] = G_in.at(1);
        vector<MX> nlp_out(NL_NUM_OUT);
        nlp_out[NL_G] = G(G_in).at(0);
        return MXFunction(nlp_in, nlp_out);
      }
    } else {
      // Standard (constrained) NLP

      // SXFunction if both functions are SXFunction
      if (is_a<SXFunction>(F) && is_a<SXFunction>(G)) {
        vector<SX> nlp_in(NL_NUM_IN), nlp_out(NL_NUM_OUT);
        SXFunction F_sx = shared_cast<SXFunction>(F);
        SXFunction G_sx = shared_cast<SXFunction>(G);
        nlp_in[NL_X] = G_sx.inputExpr(0);
        if (G_sx.getNumInputs()>1) {
          nlp_in[NL_P] = G_sx.inputExpr(1);
        } else {
          nlp_in[NL_P] = SX::sym("p", 1, 0);
        }

        // Expression for f and g
        nlp_out[NL_G] = G_sx.outputExpr(0);
        nlp_out[NL_F] = substitute(F_sx.outputExpr(), F_sx.inputExpr(), G_sx.inputExpr()).front();

        return SXFunction(nlp_in, nlp_out);
      } else { // MXFunction otherwise
        vector<MX> nlp_in(NL_NUM_IN), nlp_out(NL_NUM_OUT);

        // Try to cast into MXFunction
        MXFunction F_mx = shared_cast<MXFunction>(F);
        MXFunction G_mx = shared_cast<MXFunction>(G);

        // Convert to MX if cast failed and make sure that they
        // use the same expressions if cast was successful
        if (!G_mx.isNull()) {
          nlp_in[NL_X] = G_mx.inputExpr(0);
          if (G_mx.getNumInputs()>1) {
            nlp_in[NL_P] = G_mx.inputExpr(1);
          } else {
            nlp_in[NL_P] = MX::sym("p", 1, 0);
          }
          nlp_out[NL_G] = G_mx.outputExpr(0);
          if (!F_mx.isNull()) { // Both are MXFunction, make sure they use the same variables
            nlp_out[NL_F] = substitute(F_mx.outputExpr(), F_mx.inputExpr(),
                                       G_mx.inputExpr()).front();
          } else { // G_ but not F_ MXFunction
            nlp_out[NL_F] = F(G_mx.inputExpr()).front();
          }
        } else {
          if (!F_mx.isNull()) { // F but not G MXFunction
            nlp_in[NL_X] = F_mx.inputExpr(0);
            if (F_mx.getNumInputs()>1) {
              nlp_in[NL_P] = F_mx.inputExpr(1);
            } else {
              nlp_in[NL_P] = MX::sym("p", 1, 0);
            }
            nlp_out[NL_F] = F_mx.outputExpr(0);
            nlp_out[NL_G] = G(F_mx.inputExpr()).front();
          } else { // None of them MXFunction
            vector<MX> FG_in = G.symbolicInput();
            nlp_in[NL_X] = FG_in.at(0);
            if (FG_in.size()>1) nlp_in[NL_P] = FG_in.at(1);
            nlp_out[NL_G] = G(FG_in).front();
            nlp_out[NL_F] = F(FG_in).front();
          }
        }
        return MXFunction(nlp_in, nlp_out);
      } // SXFunction/MXFunction
    } // constrained/unconstrained
  }
void DirectCollocationInternal::init(){
  // Initialize the base classes
  OCPSolverInternal::init();
  
  // Free parameters currently not supported
  casadi_assert_message(np_==0, "Not implemented");

  // Interpolation order
  deg_ = getOption("interpolation_order");

  // All collocation time points
  std::vector<double> tau_root = collocationPoints(deg_,getOption("collocation_scheme"));

  // Size of the finite elements
  double h = tf_/nk_;

  // Coefficients of the collocation equation
  vector<vector<MX> > C(deg_+1,vector<MX>(deg_+1));

  // Coefficients of the collocation equation as DMatrix
  DMatrix C_num = DMatrix::zeros(deg_+1,deg_+1);

  // Coefficients of the continuity equation
  vector<MX> D(deg_+1);

  // Coefficients of the collocation equation as DMatrix
  DMatrix D_num = DMatrix::zeros(deg_+1);

  // Collocation point
  SX tau = SX::sym("tau");

  // For all collocation points
  for(int j=0; j<deg_+1; ++j){
    // Construct Lagrange polynomials to get the polynomial basis at the collocation point
    SX L = 1;
    for(int j2=0; j2<deg_+1; ++j2){
      if(j2 != j){
        L *= (tau-tau_root[j2])/(tau_root[j]-tau_root[j2]);
      }
    }

    SXFunction lfcn(tau,L);
    lfcn.init();

    // Evaluate the polynomial at the final time to get the coefficients of the continuity equation
    lfcn.setInput(1.0);
    lfcn.evaluate();
    D[j] = lfcn.output();
    D_num(j) = lfcn.output();

    // Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
    Function tfcn = lfcn.tangent();
    tfcn.init();
    for(int j2=0; j2<deg_+1; ++j2){
      tfcn.setInput(tau_root[j2]);
      tfcn.evaluate();
      C[j][j2] = tfcn.output();
      C_num(j,j2) = tfcn.output();
    }
  }

  C_num(std::vector<int>(1,0),ALL) = 0;
  C_num(0,0)   = 1;

  // All collocation time points
  vector<vector<double> > T(nk_);
  for(int k=0; k<nk_; ++k){
          T[k].resize(deg_+1);
          for(int j=0; j<=deg_; ++j){
                  T[k][j] = h*(k + tau_root[j]);
          }
  }

  // Total number of variables
  int nlp_nx = 0;
  nlp_nx += nk_*(deg_+1)*nx_;   // Collocated states
  nlp_nx += nk_*nu_;            // Parametrized controls
  nlp_nx += nx_;                       // Final state

  // NLP variable vector
  MX nlp_x = MX::sym("x",nlp_nx);
  int offset = 0;

  // Get collocated states and parametrized control
  vector<vector<MX> > X(nk_+1);
  vector<MX> U(nk_);
  for(int k=0; k<nk_; ++k){
    // Collocated states
        X[k].resize(deg_+1);
    for(int j=0; j<=deg_; ++j){
        // Get the expression for the state vector
        X[k][j] = nlp_x[Slice(offset,offset+nx_)];
        offset += nx_;
    }

    // Parametrized controls
    U[k] = nlp_x[Slice(offset,offset+nu_)];
    offset += nu_;
  }

  // State at end time
  X[nk_].resize(1);
  X[nk_][0] = nlp_x[Slice(offset,offset+nx_)];
  offset += nx_;
  casadi_assert(offset==nlp_nx);

  // Constraint function for the NLP
  vector<MX> nlp_g;

  // Objective function
  MX nlp_j = 0;

  // For all finite elements
  for(int k=0; k<nk_; ++k){

    // For all collocation points
    for(int j=1; j<=deg_; ++j){

        // Get an expression for the state derivative at the collocation point
        MX xp_jk = 0;
        for(int r=0; r<=deg_; ++r){
            xp_jk += C[r][j]*X[k][r];
        }

        // Add collocation equations to the NLP
        MX fk = ffcn_.call(daeIn("x",X[k][j],"p",U[k]))[DAE_ODE];
        nlp_g.push_back(h*fk - xp_jk);
    }

    // Get an expression for the state at the end of the finite element
    MX xf_k = 0;
    for(int r=0; r<=deg_; ++r){
        xf_k += D[r]*X[k][r];
    }

    // Add continuity equation to NLP
    nlp_g.push_back(X[k+1][0] - xf_k);

    // Add path constraints
    if(nh_>0){
      MX pk = cfcn_.call(daeIn("x",X[k+1][0],"p",U[k])).at(0);
      nlp_g.push_back(pk);
    }

    // Add integral objective function term
        //    [Jk] = lfcn.call([X[k+1,0], U[k]])
        //    nlp_j += Jk
  }

  // Add end cost
  MX Jk = mfcn_.call(mayerIn("x",X[nk_][0])).at(0);
  nlp_j += Jk;

  // Objective function of the NLP
  nlp_ = MXFunction(nlpIn("x",nlp_x), nlpOut("f",nlp_j,"g",vertcat(nlp_g)));

  // Get the NLP creator function
  NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
  
  // Allocate an NLP solver
  nlp_solver_ = nlp_solver_creator(nlp_);
  
  // Pass options
  if(hasSetOption("nlp_solver_options")){
    const Dictionary& nlp_solver_options = getOption("nlp_solver_options");
    nlp_solver_.setOption(nlp_solver_options);
  }
  
  // Initialize the solver
  nlp_solver_.init();
}
Exemplo n.º 19
0
  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 DirectMultipleShootingInternal::init(){
  // Initialize the base classes
  OCPSolverInternal::init();

  // Create an integrator instance
  integratorCreator integrator_creator = getOption("integrator");
  integrator_ = integrator_creator(ffcn_,Function());
  if(hasSetOption("integrator_options")){
    integrator_.setOption(getOption("integrator_options"));
  }

  // Set t0 and tf
  integrator_.setOption("t0",0);
  integrator_.setOption("tf",tf_/nk_);
  integrator_.init();
  
  // Path constraints present?
  bool path_constraints = nh_>0;
  
  // Count the total number of NLP variables
  int NV = np_ + // global parameters
           nx_*(nk_+1) + // local state
           nu_*nk_; // local control
           
  // Declare variable vector for the NLP
  // The structure is as follows:
  // np x 1  (parameters)
  // ------
  // nx x 1  (states at time i=0)
  // nu x 1  (controls in interval i=0)
  // ------
  // nx x 1  (states at time i=1)
  // nu x 1  (controls in interval i=1)
  // ------
  // .....
  // ------
  // nx x 1  (states at time i=nk)
  
  MX V = MX::sym("V",NV);

  // Global parameters
  MX P = V(Slice(0,np_));

  // offset in the variable vector
  int v_offset=np_; 
  
  // Disretized variables for each shooting node
  vector<MX> X(nk_+1), U(nk_);
  for(int k=0; k<=nk_; ++k){ // interior nodes
    // Local state
    X[k] = V[Slice(v_offset,v_offset+nx_)];
    v_offset += nx_;
    
    // Variables below do not appear at the end point
    if(k==nk_) break;
    
    // Local control
    U[k] = V[Slice(v_offset,v_offset+nu_)];
    v_offset += nu_;
  }
  
  // Make sure that the size of the variable vector is consistent with the number of variables that we have referenced
  casadi_assert(v_offset==NV);

  // Input to the parallel integrator evaluation
  vector<vector<MX> > int_in(nk_);
  for(int k=0; k<nk_; ++k){
    int_in[k].resize(INTEGRATOR_NUM_IN);
    int_in[k][INTEGRATOR_P] = vertcat(P,U[k]);
    int_in[k][INTEGRATOR_X0] = X[k];
  }

  // Input to the parallel function evaluation
  vector<vector<MX> > fcn_in(nk_);
  for(int k=0; k<nk_; ++k){
    fcn_in[k].resize(DAE_NUM_IN);
    fcn_in[k][DAE_T] = (k*tf_)/nk_;
    fcn_in[k][DAE_P] = vertcat(P,U.at(k));
    fcn_in[k][DAE_X] = X[k];
  }

  // Options for the parallelizer
  Dictionary paropt;
  
  // Transmit parallelization mode
  if(hasSetOption("parallelization"))
    paropt["parallelization"] = getOption("parallelization");
  
  // Evaluate function in parallel
  vector<vector<MX> > pI_out = integrator_.callParallel(int_in,paropt);

  // Evaluate path constraints in parallel
  vector<vector<MX> > pC_out;
  if(path_constraints)
    pC_out = cfcn_.callParallel(fcn_in,paropt);
  
  //Constraint function
  vector<MX> gg(2*nk_);

  // Collect the outputs
  for(int k=0; k<nk_; ++k){
    //append continuity constraints
    gg[2*k] = pI_out[k][INTEGRATOR_XF] - X[k+1];
    
    // append the path constraints
    if(path_constraints)
      gg[2*k+1] = pC_out[k][0];
  }

  // Terminal constraints
  MX g = vertcat(gg);

  // Objective function
  MX f;
  if (mfcn_.getNumInputs()==1) {
    f = mfcn_(X.back()).front();
  } else {
    vector<MX> mfcn_argin(MAYER_NUM_IN); 
    mfcn_argin[MAYER_X] = X.back();
    mfcn_argin[MAYER_P] = P;
    f = mfcn_.call(mfcn_argin).front();
  }

  // NLP
  nlp_ = MXFunction(nlpIn("x",V),nlpOut("f",f,"g",g));
  nlp_.setOption("ad_mode","forward");
  nlp_.init();
  
  // Get the NLP creator function
  NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
  
  // Allocate an NLP solver
  nlp_solver_ = nlp_solver_creator(nlp_);
  
  // Pass user options
  if(hasSetOption("nlp_solver_options")){
    const Dictionary& nlp_solver_options = getOption("nlp_solver_options");
    nlp_solver_.setOption(nlp_solver_options);
  }
  
  // Initialize the solver
  nlp_solver_.init();
}
Exemplo n.º 21
0
void DirectCollocationInternal::init(){
  // Initialize the base classes
  OCPSolverInternal::init();
  
  // Free parameters currently not supported
  casadi_assert_message(np_==0, "Not implemented");

  // Legendre collocation points
  double legendre_points[][6] = {
    {0},
    {0,0.500000},
    {0,0.211325,0.788675},
    {0,0.112702,0.500000,0.887298},
    {0,0.069432,0.330009,0.669991,0.930568},
    {0,0.046910,0.230765,0.500000,0.769235,0.953090}};

  // Radau collocation points
  double radau_points[][6] = {
    {0},
    {0,1.000000},
    {0,0.333333,1.000000},
    {0,0.155051,0.644949,1.000000},
    {0,0.088588,0.409467,0.787659,1.000000},
    {0,0.057104,0.276843,0.583590,0.860240,1.000000}};

  // Read options
  bool use_radau;
  if(getOption("collocation_scheme")=="radau"){
    use_radau = true;
  } else if(getOption("collocation_scheme")=="legendre"){
    use_radau = false;
  }

  // Interpolation order
  deg_ = getOption("interpolation_order");

  // All collocation time points
  double* tau_root = use_radau ? radau_points[deg_] : legendre_points[deg_];

  // Size of the finite elements
  double h = tf_/nk_;

  // Coefficients of the collocation equation
  vector<vector<MX> > C(deg_+1,vector<MX>(deg_+1));

  // Coefficients of the collocation equation as DMatrix
  DMatrix C_num = DMatrix(deg_+1,deg_+1,0);

  // Coefficients of the continuity equation
  vector<MX> D(deg_+1);

  // Coefficients of the collocation equation as DMatrix
  DMatrix D_num = DMatrix(deg_+1,1,0);

  // Collocation point
  SXMatrix tau = ssym("tau");

  // For all collocation points
  for(int j=0; j<deg_+1; ++j){
    // Construct Lagrange polynomials to get the polynomial basis at the collocation point
    SXMatrix L = 1;
    for(int j2=0; j2<deg_+1; ++j2){
      if(j2 != j){
        L *= (tau-tau_root[j2])/(tau_root[j]-tau_root[j2]);
      }
    }

    SXFunction lfcn(tau,L);
    lfcn.init();

    // Evaluate the polynomial at the final time to get the coefficients of the continuity equation
    lfcn.setInput(1.0);
    lfcn.evaluate();
    D[j] = lfcn.output();
    D_num(j) = lfcn.output();

    // Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
    for(int j2=0; j2<deg_+1; ++j2){
      lfcn.setInput(tau_root[j2]);
      lfcn.setFwdSeed(1.0);
      lfcn.evaluate(1,0);
      C[j][j2] = lfcn.fwdSens();
      C_num(j,j2) = lfcn.fwdSens();
    }
  }

  C_num(std::vector<int>(1,0),ALL) = 0;
  C_num(0,0)   = 1;

  // All collocation time points
  vector<vector<double> > T(nk_);
  for(int k=0; k<nk_; ++k){
	  T[k].resize(deg_+1);
	  for(int j=0; j<=deg_; ++j){
		  T[k][j] = h*(k + tau_root[j]);
	  }
  }

  // Total number of variables
  int nlp_nx = 0;
  nlp_nx += nk_*(deg_+1)*nx_;   // Collocated states
  nlp_nx += nk_*nu_;            // Parametrized controls
  nlp_nx += nx_;               	// Final state

  // NLP variable vector
  MX nlp_x = msym("x",nlp_nx);
  int offset = 0;

  // Get collocated states and parametrized control
  vector<vector<MX> > X(nk_+1);
  vector<MX> U(nk_);
  for(int k=0; k<nk_; ++k){
    // Collocated states
	X[k].resize(deg_+1);
    for(int j=0; j<=deg_; ++j){
        // Get the expression for the state vector
        X[k][j] = nlp_x[Slice(offset,offset+nx_)];
        offset += nx_;
    }

    // Parametrized controls
    U[k] = nlp_x[Slice(offset,offset+nu_)];
    offset += nu_;
  }

  // State at end time
  X[nk_].resize(1);
  X[nk_][0] = nlp_x[Slice(offset,offset+nx_)];
  offset += nx_;
  casadi_assert(offset==nlp_nx);

  // Constraint function for the NLP
  vector<MX> nlp_g;

  // Objective function
  MX nlp_j = 0;

  // For all finite elements
  for(int k=0; k<nk_; ++k){

    // For all collocation points
    for(int j=1; j<=deg_; ++j){

        // Get an expression for the state derivative at the collocation point
        MX xp_jk = 0;
        for(int r=0; r<=deg_; ++r){
            xp_jk += C[r][j]*X[k][r];
        }

        // Add collocation equations to the NLP
        MX fk = ffcn_.call(daeIn("x",X[k][j],"p",U[k]))[DAE_ODE];
        nlp_g.push_back(h*fk - xp_jk);
    }

    // Get an expression for the state at the end of the finite element
    MX xf_k = 0;
    for(int r=0; r<=deg_; ++r){
        xf_k += D[r]*X[k][r];
    }

    // Add continuity equation to NLP
    nlp_g.push_back(X[k+1][0] - xf_k);

    // Add path constraints
    if(nh_>0){
      MX pk = cfcn_.call(daeIn("x",X[k+1][0],"p",U[k])).at(0);
      nlp_g.push_back(pk);
    }

    // Add integral objective function term
	//    [Jk] = lfcn.call([X[k+1,0], U[k]])
	//    nlp_j += Jk
  }

  // Add end cost
  MX Jk = mfcn_.call(mayerIn("x",X[nk_][0])).at(0);
  nlp_j += Jk;

  // Objective function of the NLP
  F_ = MXFunction(nlp_x, nlp_j);

  // Nonlinear constraint function
  G_ = MXFunction(nlp_x, vertcat(nlp_g));

  // Get the NLP creator function
  NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
  
  // Allocate an NLP solver
  nlp_solver_ = nlp_solver_creator(F_,G_,FX(),FX());
  
  // Pass options
  if(hasSetOption("nlp_solver_options")){
    const Dictionary& nlp_solver_options = getOption("nlp_solver_options");
    nlp_solver_.setOption(nlp_solver_options);
  }
  
  // Initialize the solver
  nlp_solver_.init();
}
void DirectSingleShootingInternal::init(){
  // Initialize the base classes
  OCPSolverInternal::init();

  // Create an integrator instance
  integratorCreator integrator_creator = getOption("integrator");
  integrator_ = integrator_creator(ffcn_,FX());
  if(hasSetOption("integrator_options")){
    integrator_.setOption(getOption("integrator_options"));
  }

  // Set t0 and tf
  integrator_.setOption("t0",0);
  integrator_.setOption("tf",tf_/nk_);
  integrator_.init();
  
  // Path constraints present?
  bool path_constraints = nh_>0;
  
  // Count the total number of NLP variables
  int NV = np_ + // global parameters
           nx_ + // initial state
           nu_*nk_; // local control
           
  // Declare variable vector for the NLP
  // The structure is as follows:
  // np x 1  (parameters)
  // ------
  // nx x 1  (states at time i=0)
  // ------
  // nu x 1  (controls in interval i=0)
  // .....
  // nx x 1  (controls in interval i=nk-1)
  
  MX V = msym("V",NV);
  int offset = 0;

  // Global parameters
  MX P = V[Slice(0,np_)];
  offset += np_;

  // Initial state
  MX X0 = V[Slice(offset,offset+nx_)];
  offset += nx_;
  
  // Control for each shooting interval
  vector<MX> U(nk_);
  for(int k=0; k<nk_; ++k){ // interior nodes
    U[k] = V[range(offset,offset+nu_)];
    offset += nu_;
  }
  
  // Make sure that the size of the variable vector is consistent with the number of variables that we have referenced
  casadi_assert(offset==NV);

  // Current state
  MX X = X0;

  // Objective
  MX nlp_j = 0;

  // Constraints
  vector<MX> nlp_g;
  nlp_g.reserve(nk_*(path_constraints ? 2 : 1));
  
  // For all shooting nodes
  for(int k=0; k<nk_; ++k){
    // Integrate
    vector<MX> int_out = integrator_.call(integratorIn("x0",X,"p",vertcat(P,U[k])));

    // Store expression for state trajectory
    X = int_out[INTEGRATOR_XF];
    
    // Add constraints on the state
    nlp_g.push_back(X);

    // Add path constraints
    if(path_constraints){
      vector<MX> cfcn_out = cfcn_.call(daeIn("x",X,"p",U[k])); // TODO: Change signature of cfcn_: remove algebraic variable, add control
      nlp_g.push_back(cfcn_out.at(0));
    }
  }

  // Terminal constraints
  G_ = MXFunction(V,vertcat(nlp_g));
  G_.setOption("name","nlp_g");
  G_.init();
  
  // Objective function
  MX jk = mfcn_.call(mayerIn("x",X,"p",P)).at(0);
  nlp_j += jk;
  F_ = MXFunction(V,nlp_j);
  F_.setOption("name","nlp_j");
  
  // Get the NLP creator function
  NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
  
  // Allocate an NLP solver
  nlp_solver_ = nlp_solver_creator(F_,G_,FX(),FX());
  
  // Pass options
  if(hasSetOption("nlp_solver_options")){
    const Dictionary& nlp_solver_options = getOption("nlp_solver_options");
    nlp_solver_.setOption(nlp_solver_options);
  }
  
  // Initialize the solver
  nlp_solver_.init();
}
  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() << ".");
    }
  }
Exemplo n.º 24
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);
  }
  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 CollocationIntegratorInternal::init(){
  
  // Call the base class init
  IntegratorInternal::init();
  
  // Legendre collocation points
  double legendre_points[][6] = {
    {0},
    {0,0.500000},
    {0,0.211325,0.788675},
    {0,0.112702,0.500000,0.887298},
    {0,0.069432,0.330009,0.669991,0.930568},
    {0,0.046910,0.230765,0.500000,0.769235,0.953090}};
    
  // Radau collocation points
  double radau_points[][6] = {
    {0},
    {0,1.000000},
    {0,0.333333,1.000000},
    {0,0.155051,0.644949,1.000000},
    {0,0.088588,0.409467,0.787659,1.000000},
    {0,0.057104,0.276843,0.583590,0.860240,1.000000}};

  // Read options
  bool use_radau;
  if(getOption("collocation_scheme")=="radau"){
    use_radau = true;
  } else if(getOption("collocation_scheme")=="legendre"){
    use_radau = false;
  }
  
  // Hotstart?
  hotstart_ = getOption("hotstart");
  
  // Number of finite elements
  int nk = getOption("number_of_finite_elements");
  
  // Interpolation order
  int deg = getOption("interpolation_order");

  // Assume explicit ODE
  bool explicit_ode = f_.input(DAE_XDOT).size()==0;
  
  // All collocation time points
  double* tau_root = use_radau ? radau_points[deg] : legendre_points[deg];

  // Size of the finite elements
  double h = (tf_-t0_)/nk;
  
  // MX version of the same
  MX h_mx = h;
    
  // Coefficients of the collocation equation
  vector<vector<MX> > C(deg+1,vector<MX>(deg+1));

  // Coefficients of the continuity equation
  vector<MX> D(deg+1);

  // Collocation point
  SXMatrix tau = ssym("tau");

  // For all collocation points
  for(int j=0; j<deg+1; ++j){
    // Construct Lagrange polynomials to get the polynomial basis at the collocation point
    SXMatrix L = 1;
    for(int j2=0; j2<deg+1; ++j2){
      if(j2 != j){
        L *= (tau-tau_root[j2])/(tau_root[j]-tau_root[j2]);
      }
    }
    
    SXFunction lfcn(tau,L);
    lfcn.init();
  
    // Evaluate the polynomial at the final time to get the coefficients of the continuity equation
    lfcn.setInput(1.0);
    lfcn.evaluate();
    D[j] = lfcn.output();

    // Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
    for(int j2=0; j2<deg+1; ++j2){
      lfcn.setInput(tau_root[j2]);
      lfcn.setFwdSeed(1.0);
      lfcn.evaluate(1,0);
      C[j][j2] = lfcn.fwdSens();
    }
  }
  
  // Initial state
  MX X0("X0",nx_);
  
  // Parameters
  MX P("P",np_);
  
  // Backward state
  MX RX0("RX0",nrx_);
  
  // Backward parameters
  MX RP("RP",nrp_);
  
  // Collocated differential states and algebraic variables
  int nX = (nk*(deg+1)+1)*(nx_+nrx_);
  int nZ = nk*deg*(nz_+nrz_);
  
  // Unknowns
  MX V("V",nX+nZ);
  int offset = 0;
  
  // Get collocated states, algebraic variables and times
  vector<vector<MX> > X(nk+1);
  vector<vector<MX> > RX(nk+1);
  vector<vector<MX> > Z(nk);
  vector<vector<MX> > RZ(nk);
  coll_time_.resize(nk+1);
  for(int k=0; k<nk+1; ++k){
    // Number of time points
    int nj = k==nk ? 1 : deg+1;
    
    // Allocate differential states expressions at the time points
    X[k].resize(nj);
    RX[k].resize(nj);
    coll_time_[k].resize(nj);

    // Allocate algebraic variable expressions at the collocation points
    if(k!=nk){
      Z[k].resize(nj-1);
      RZ[k].resize(nj-1);
    }

    // For all time points
    for(int j=0; j<nj; ++j){
      // Get expressions for the differential state
      X[k][j] = V[range(offset,offset+nx_)];
      offset += nx_;
      RX[k][j] = V[range(offset,offset+nrx_)];
      offset += nrx_;
      
      // Get the local time
      coll_time_[k][j] = h*(k + tau_root[j]);
      
      // Get expressions for the algebraic variables
      if(j>0){
        Z[k][j-1] = V[range(offset,offset+nz_)];
        offset += nz_;
        RZ[k][j-1] = V[range(offset,offset+nrz_)];
        offset += nrz_;
      }
    }
  }
  
  // Check offset for consistency
  casadi_assert(offset==V.size());

  // Constraints
  vector<MX> g;
  g.reserve(2*(nk+1));
  
  // Quadrature expressions
  MX QF = MX::zeros(nq_);
  MX RQF = MX::zeros(nrq_);
  
  // Counter
  int jk = 0;
  
  // Add initial condition
  g.push_back(X[0][0]-X0);
  
  // For all finite elements
  for(int k=0; k<nk; ++k, ++jk){
  
    // For all collocation points
    for(int j=1; j<deg+1; ++j, ++jk){
      // Get the time
      MX tkj = coll_time_[k][j];
      
      // Get an expression for the state derivative at the collocation point
      MX xp_jk = 0;
      for(int j2=0; j2<deg+1; ++j2){
        xp_jk += C[j2][j]*X[k][j2];
      }
      
      // Add collocation equations to the NLP
      vector<MX> f_in(DAE_NUM_IN);
      f_in[DAE_T] = tkj;
      f_in[DAE_P] = P;
      f_in[DAE_X] = X[k][j];
      f_in[DAE_Z] = Z[k][j-1];
      
      vector<MX> f_out;
      if(explicit_ode){
        // Assume equation of the form ydot = f(t,y,p)
        f_out = f_.call(f_in);
        g.push_back(h_mx*f_out[DAE_ODE] - xp_jk);
      } else {
        // Assume equation of the form 0 = f(t,y,ydot,p)
        f_in[DAE_XDOT] = xp_jk/h_mx;
        f_out = f_.call(f_in);
        g.push_back(f_out[DAE_ODE]);
      }
      
      // Add the algebraic conditions
      if(nz_>0){
        g.push_back(f_out[DAE_ALG]);
      }
      
      // Add the quadrature
      if(nq_>0){
        QF += D[j]*h_mx*f_out[DAE_QUAD];
      }
      
      // Now for the backward problem
      if(nrx_>0){
        
        // Get an expression for the state derivative at the collocation point
        MX rxp_jk = 0;
        for(int j2=0; j2<deg+1; ++j2){
          rxp_jk += C[j2][j]*RX[k][j2];
        }
        
        // Add collocation equations to the NLP
        vector<MX> g_in(RDAE_NUM_IN);
        g_in[RDAE_T] = tkj;
        g_in[RDAE_X] = X[k][j];
        g_in[RDAE_Z] = Z[k][j-1];
        g_in[RDAE_P] = P;
        g_in[RDAE_RP] = RP;
        g_in[RDAE_RX] = RX[k][j];
        g_in[RDAE_RZ] = RZ[k][j-1];
        
        vector<MX> g_out;
        if(explicit_ode){
          // Assume equation of the form xdot = f(t,x,p)
          g_out = g_.call(g_in);
          g.push_back(h_mx*g_out[RDAE_ODE] - rxp_jk);
        } else {
          // Assume equation of the form 0 = f(t,x,xdot,p)
          g_in[RDAE_XDOT] = xp_jk/h_mx;
          g_in[RDAE_RXDOT] = rxp_jk/h_mx;
          g_out = g_.call(g_in);
          g.push_back(g_out[RDAE_ODE]);
        }
        
        // Add the algebraic conditions
        if(nrz_>0){
          g.push_back(g_out[RDAE_ALG]);
        }
        
        // Add the backward quadrature
        if(nrq_>0){
          RQF += D[j]*h_mx*g_out[RDAE_QUAD];
        }
      }
    }
    
    // Get an expression for the state at the end of the finite element
    MX xf_k = 0;
    for(int j=0; j<deg+1; ++j){
      xf_k += D[j]*X[k][j];
    }

    // Add continuity equation to NLP
    g.push_back(X[k+1][0] - xf_k);
    
    if(nrx_>0){
      // Get an expression for the state at the end of the finite element
      MX rxf_k = 0;
      for(int j=0; j<deg+1; ++j){
        rxf_k += D[j]*RX[k][j];
      }

      // Add continuity equation to NLP
      g.push_back(RX[k+1][0] - rxf_k);
    }
  }
  
  // Add initial condition for the backward integration
  if(nrx_>0){
    g.push_back(RX[nk][0]-RX0);
  }
  
  // Constraint expression
  MX gv = vertcat(g);
    
  // Make sure that the dimension is consistent with the number of unknowns
  casadi_assert_message(gv.size()==V.size(),"Implicit function unknowns and equations do not match");

  // Nonlinear constraint function input
  vector<MX> gfcn_in(1+INTEGRATOR_NUM_IN);
  gfcn_in[0] = V;
  gfcn_in[1+INTEGRATOR_X0] = X0;
  gfcn_in[1+INTEGRATOR_P] = P;
  gfcn_in[1+INTEGRATOR_RX0] = RX0;
  gfcn_in[1+INTEGRATOR_RP] = RP;

  vector<MX> gfcn_out(1+INTEGRATOR_NUM_OUT);
  gfcn_out[0] = gv;
  gfcn_out[1+INTEGRATOR_XF] = X[nk][0];
  gfcn_out[1+INTEGRATOR_QF] = QF;
  gfcn_out[1+INTEGRATOR_RXF] = RX[0][0];
  gfcn_out[1+INTEGRATOR_RQF] = RQF;
  
  // Nonlinear constraint function
  FX gfcn = MXFunction(gfcn_in,gfcn_out);
  
  // Expand f?
  bool expand_f = getOption("expand_f");
  if(expand_f){
    gfcn.init();
    gfcn = SXFunction(shared_cast<MXFunction>(gfcn));
  }
  
  // Get the NLP creator function
  implicitFunctionCreator implicit_function_creator = getOption("implicit_solver");
  
  // Allocate an NLP solver
  implicit_solver_ = implicit_function_creator(gfcn);
  
  // Pass options
  if(hasSetOption("implicit_solver_options")){
    const Dictionary& implicit_solver_options = getOption("implicit_solver_options");
    implicit_solver_.setOption(implicit_solver_options);
  }
  
  // Initialize the solver
  implicit_solver_.init();
  
  if(hasSetOption("startup_integrator")){
    
    // Create the linear solver
    integratorCreator startup_integrator_creator = getOption("startup_integrator");
    
    // Allocate an NLP solver
    startup_integrator_ = startup_integrator_creator(f_,g_);
    
    // Pass options
    startup_integrator_.setOption("number_of_fwd_dir",0); // not needed
    startup_integrator_.setOption("number_of_adj_dir",0); // not needed
    startup_integrator_.setOption("t0",coll_time_.front().front());
    startup_integrator_.setOption("tf",coll_time_.back().back());
    if(hasSetOption("startup_integrator_options")){
      const Dictionary& startup_integrator_options = getOption("startup_integrator_options");
      startup_integrator_.setOption(startup_integrator_options);
    }
    
    // Initialize the startup integrator
    startup_integrator_.init();
  }

  // Mark the system not yet integrated
  integrated_once_ = false;
}