コード例 #1
0
void ImplicitFunctionInternal::init(){
  // Initialize the residual function
  if(!f_.isInit()) f_.init();
  
  // Allocate inputs
  setNumInputs(f_.getNumInputs()-1);
  for(int i=0; i<getNumInputs(); ++i){
    input(i) = f_.input(i+1);
  }
  
  // Allocate outputs
  setNumOutputs(f_.getNumOutputs());
  output(0) = f_.input(0);
  for(int i=1; i<getNumOutputs(); ++i){
    output(i) = f_.output(i);
  }

  // Call the base class initializer
  FXInternal::init();

  // Number of equations
  N_ = output().size();

  // Generate Jacobian if not provided
  if(J_.isNull()) J_ = f_.jacobian(0,0);
  J_.init();
  
  casadi_assert_message(J_.output().size1()==J_.output().size2(),"ImplicitFunctionInternal::init: the jacobian must be square but got " << J_.output().dimString());
  
  casadi_assert_message(!isSingular(J_.output().sparsity()),"ImplicitFunctionInternal::init: singularity - the jacobian is structurally rank-deficient. sprank(J)=" << sprank(J_.output()) << " (in stead of "<< J_.output().size1() << ")");
  
  // Get the linear solver creator function
  if(linsol_.isNull() && hasSetOption("linear_solver")){
    linearSolverCreator linear_solver_creator = getOption("linear_solver");
  
    // Allocate an NLP solver
    linsol_ = linear_solver_creator(CRSSparsity());
  
    // Pass options
    if(hasSetOption("linear_solver_options")){
      const Dictionary& linear_solver_options = getOption("linear_solver_options");
      linsol_.setOption(linear_solver_options);
    }
  }
  
  // Initialize the linear solver, if provided
  if(!linsol_.isNull()){
    linsol_.setSparsity(J_.output().sparsity());
    linsol_.init();
  }
    
  // Allocate memory for directional derivatives
  ImplicitFunctionInternal::updateNumSens(false);

}
コード例 #2
0
ファイル: implicit_to_nlp.cpp プロジェクト: cfpperche/casadi
  void QpToImplicit::init() {
    // Call the base class initializer
    ImplicitFunctionInternal::init();

    // Free variable in the NLP
    MX u = MX::sym("u", input(iin_).sparsity());

    // So that we can pass it on to createParent
    std::vector<Sparsity> sps;
    for (int i=0; i<getNumInputs(); ++i)
      if (i!=iin_) sps.push_back(input(i).sparsity());

    // u groups all parameters in an MX
    std::vector< MX > inputs;
    MX p = createParent(sps, inputs);

    // Dummy NLP objective
    MX nlp_f = 0;

    // NLP constraints
    std::vector< MX > args_call(getNumInputs());
    args_call[iin_] = u;
    for (int i=0, i2=0; i<getNumInputs(); ++i)
      if (i!=iin_) args_call[i] = inputs[i2++];
    MX nlp_g = f_.call(args_call).at(iout_);

    // We're going to use two-argument objective and constraints to allow the use of parameters
    MXFunction nlp(nlpIn("x", u, "p", p), nlpOut("f", nlp_f, "g", nlp_g));

    // Create an NlpSolver instance
    solver_ = NlpSolver(getOption(solvername()), nlp);
    if (hasSetOption(optionsname())) solver_.setOption(getOption(optionsname()));
    solver_.init();
  }
コード例 #3
0
ファイル: lr_dple_to_dple.cpp プロジェクト: BrechtBa/casadi
  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();
  }
コード例 #4
0
  void ImplicitFixedStepIntegratorInternal::init() {
    // Call the base class init
    FixedStepIntegratorInternal::init();

    // Get the NLP creator function
    std::string implicit_function_name = getOption("implicit_solver");

    // Allocate an NLP solver
    implicit_solver_ = ImplicitFunction(implicit_function_name, F_, Function(), LinearSolver());
    implicit_solver_.setOption("name", string(getOption("name")) + "_implicit_solver");
    implicit_solver_.setOption("implicit_input", DAE_Z);
    implicit_solver_.setOption("implicit_output", DAE_ALG);

    // 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();

    // Allocate a root-finding solver for the backward problem
    if (nRZ_>0) {

      // Get the NLP creator function
      std::string backward_implicit_function_name = getOption("implicit_solver");

      // Allocate an NLP solver
      backward_implicit_solver_ = ImplicitFunction(backward_implicit_function_name,
                                                   G_, Function(), LinearSolver());
      backward_implicit_solver_.setOption("name",
                                          string(getOption("name")) + "_backward_implicit_solver");
      backward_implicit_solver_.setOption("implicit_input", RDAE_RZ);
      backward_implicit_solver_.setOption("implicit_output", RDAE_ALG);

      // Pass options
      if (hasSetOption("implicit_solver_options")) {
        const Dictionary& backward_implicit_solver_options = getOption("implicit_solver_options");
        backward_implicit_solver_.setOption(backward_implicit_solver_options);
      }

      // Initialize the solver
      backward_implicit_solver_.init();
    }
  }
コード例 #5
0
  void WorhpInternal::init(){

    // Call the init method of the base class
    NLPSolverInternal::init();

    if (hasSetOption("Ares")) {
      std::vector<int> ares = getOption("Ares");
      std::copy(ares.begin(),ares.begin()+NAres,worhp_p_.Ares);
    }
  
    // Read options
    passOptions();
  
    // Exact Hessian?
    exact_hessian_ = getOption("UserHM");

    // Get/generate required functions
    gradF();
    jacG();
    if(exact_hessian_){ // does not appear to work
      hessLag();
    }
    
    // Update status?
    status_[TerminateSuccess]="TerminateSuccess";
    status_[OptimalSolution]="OptimalSolution";
    status_[SearchDirectionZero]="SearchDirectionZero";
    status_[SearchDirectionSmall]="SearchDirectionSmall";
    status_[StationaryPointFound]="StationaryPointFound";
    status_[AcceptableSolution]="AcceptableSolution";
    status_[AcceptablePrevious]="AcceptablePrevious";
    status_[FritzJohn]="FritzJohn";
    status_[NotDiffable]="NotDiffable";
    status_[Unbounded]="Unbounded";
    status_[FeasibleSolution]="FeasibleSolution";
    status_[LowPassFilterOptimal]="LowPassFilterOptimal";
    status_[LowPassFilterAcceptable]="LowPassFilterAcceptable";
    status_[TerminateError]="TerminateError";
    status_[InitError]="InitError";
    status_[DataError]="DataError";
    status_[MaxCalls]="MaxCalls";
    status_[MaxIter]="MaxIter";
    status_[MinimumStepsize]="MinimumStepsize";
    status_[QPerror]="QPerror";
    status_[ProblemInfeasible]="ProblemInfeasible";
    status_[GroupsComposition]="GroupsComposition";
    status_[TooBig]="TooBig";
    status_[Timeout]="Timeout";
    status_[FDError]="FDError";
    status_[LocalInfeas]="LocalInfeas";
    status_[LicenseError]="LicenseError. Please set the WORHP_LICENSE_FILE environmental variable with the full path to the license file";
    status_[TerminatedByUser]="TerminatedByUser";
    status_[FunctionErrorF]="FunctionErrorF";
    status_[FunctionErrorG]="FunctionErrorG";
    status_[FunctionErrorDF]="FunctionErrorDF";
    status_[FunctionErrorDG]="FunctionErrorDG";
    status_[FunctionErrorHM]="FunctionErrorHM";
  }
コード例 #6
0
ファイル: shell_compiler.cpp プロジェクト: jgillis/casadi
  void ShellCompiler::init() {
    // Initialize the base classes
    CompilerInternal::init();

    // Read options
    string compiler = getOption("compiler").toString();
    string compiler_setup = getOption("compiler_setup").toString();
    vector<string> flags;
    if (hasSetOption("flags")) flags = getOption("flags");

    // Construct the compiler command
    stringstream cmd;
    cmd << compiler << " " << compiler_setup;
    for (vector<string>::const_iterator i=flags.begin(); i!=flags.end(); ++i) {
      cmd << " " << *i;
    }

    // C/C++ source file
    cmd << " " << name_;

    // Name of temporary file
#ifdef HAVE_MKSTEMPS
    // Preferred solution
    char bin_name[] = "tmp_casadi_compiler_shell_XXXXXX.so";
    if (mkstemps(bin_name, 3) == -1) {
      casadi_error("Failed to create a temporary file name");
    }
    bin_name_ = bin_name;
#else
    // Fallback, may result in deprecation warnings
    char* bin_name = tempnam(0, "tmp_casadi_compiler_shell_");
    bin_name_ = bin_name;
    free(bin_name);
#endif

    // Have relative paths start with ./
    if (bin_name_.at(0)!='/') {
      bin_name_ = "./" + bin_name_;
    }

    // Temporary file
    cmd << " -o " << bin_name_;

    // Compile into a shared library
    if (system(cmd.str().c_str())) {
      casadi_error("Compilation failed. Tried \"" + cmd.str() + "\"");
    }

    // Load shared library
    handle_ = dlopen(bin_name_.c_str(), RTLD_LAZY);
    casadi_assert_message(handle_!=0, "CommonExternal: Cannot open function: "
                          << bin_name_ << ". error code: "<< dlerror());
    // reset error
    dlerror();
  }
コード例 #7
0
ファイル: lp_to_qp.cpp プロジェクト: ghorn/debian-casadi
  void LpToQp::init() {
    // Initialize the base classes
    LpSolverInternal::init();

    // Create a QpSolver instance
    solver_ = QpSolver(getOption(solvername()),
                       qpStruct("h", Sparsity::sparse(n_, n_),
                                "a", input(LP_SOLVER_A).sparsity()));
    if (hasSetOption(optionsname())) solver_.setOption(getOption(optionsname()));
    solver_.init();
  }
コード例 #8
0
ファイル: newton.cpp プロジェクト: cfpperche/casadi
  void Newton::init() {

    // Call the base class initializer
    ImplicitFunctionInternal::init();

    casadi_assert_message(f_.getNumInputs()>0,
                          "Newton: the supplied f must have at least one input.");
    casadi_assert_message(!linsol_.isNull(),
                          "Newton::init: linear_solver must be supplied");

    if (hasSetOption("max_iter"))
      max_iter_ = getOption("max_iter");

    if (hasSetOption("abstol"))
      abstol_ = getOption("abstol");

    if (hasSetOption("abstolStep"))
      abstolStep_ = getOption("abstolStep");

    print_iteration_ = getOption("print_iteration");

  }
コード例 #9
0
ファイル: qp_to_qcqp.cpp プロジェクト: BrechtBa/casadi
  void QpToQcqp::init() {
    // Initialize the base classes
    QpSolverInternal::init();

    Dict options;
    if (hasSetOption(optionsname())) options = getOption(optionsname());
    options = OptionsFunctionality::addOptionRecipe(options, "qp");

    // Create an QcqpSolver instance
    solver_ = QcqpSolver("qcqpsolver", getOption(solvername()),
                         make_map("h", input(QP_SOLVER_H).sparsity(),
                                  "p", Sparsity(n_, 0),
                                  "a", input(QP_SOLVER_A).sparsity()),
                         options);
  }
コード例 #10
0
ファイル: lr_dple_to_lr_dle.cpp プロジェクト: BrechtBa/casadi
  void DpleToDle::init() {

    DleInternal::init();

    // Create an dplesolver instance
    std::string dplesolver_name = getOption("dple_solver");
    dplesolver_ = DpleSolver(dplesolver_name, dpleStruct("a",
      std::vector<Sparsity>(1,A_), "v",std::vector<Sparsity>(1,V_),"c",std::vector<Sparsity>(1,C_),"h",std::vector<Sparsity>(1,H_))
    );

    if (hasSetOption("dple")) {
      dplesolver_.setOption(getOption("dple"));
    }

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

  }
コード例 #11
0
ファイル: qcqp_qp_internal.cpp プロジェクト: tmmsartor/casadi
  void QCQPQPInternal::init() {

    QpSolverInternal::init();

    // Create an qcqpsolver instance
    std::string qcqpsolver_name = getOption("qcqp_solver");
    qcqpsolver_ = QcqpSolver(qcqpsolver_name,
                             qcqpStruct("h", input(QP_SOLVER_H).sparsity(),
                                        "p", Sparsity::sparse(n_, 0),
                                        "a", input(QP_SOLVER_A).sparsity()));
    qcqpsolver_.setQPOptions();
    if (hasSetOption("qcqp_solver_options")) {
      qcqpsolver_.setOption(getOption("qcqp_solver_options"));
    }

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

  }
コード例 #12
0
ファイル: qcqp_to_socp.cpp プロジェクト: cfpperche/casadi
  void QcqpToSocp::init() {
    // Initialize the base classes
    QcqpSolverInternal::init();

    // Collection of sparsities that will make up SOCP_SOLVER_G
    std::vector<Sparsity> socp_g;

    // Allocate Cholesky solvers
    cholesky_.push_back(LinearSolver("csparsecholesky", st_[QCQP_STRUCT_H]));
    for (int i=0;i<nq_;++i) {
      cholesky_.push_back(
        LinearSolver("csparsecholesky",
          DMatrix(st_[QCQP_STRUCT_P])(range(i*n_, (i+1)*n_), ALL).sparsity()));
    }

    for (int i=0;i<nq_+1;++i) {
      // Initialize Cholesky solve
      cholesky_[i].init();

      // Harvest Cholsesky sparsity patterns
      // Note that we add extra scalar to make room for the epigraph-reformulation variable
      socp_g.push_back(blkdiag(
        cholesky_[i].getFactorizationSparsity(false), Sparsity::dense(1, 1)));
    }

    // Create an SocpSolver instance
    solver_ = SocpSolver(getOption(solvername()),
                         socpStruct("g", horzcat(socp_g),
                                    "a", horzcat(input(QCQP_SOLVER_A).sparsity(),
                                                 Sparsity::sparse(nc_, 1))));
    //solver_.setQCQPOptions();
    if (hasSetOption(optionsname())) solver_.setOption(getOption(optionsname()));

    std::vector<int> ni(nq_+1);
    for (int i=0;i<nq_+1;++i) {
      ni[i] = n_+1;
    }
    solver_.setOption("ni", ni);

    // Initialize the SocpSolver
    solver_.init();
  }
コード例 #13
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();
  }
コード例 #14
0
 bool IpoptInternal::get_starting_point(int n, bool init_x, double* x,
                                        bool init_z, double* z_L, double* z_U,
                                        int m, bool init_lambda,
                                        double* lambda)
 {
   try {
     bool warmstart = hasSetOption("warm_start_init_point") && getOption("warm_start_init_point")=="yes";
     //casadi_assert_warning(init_x,"Not initializing x");
     if (warmstart) {
       //casadi_assert_warning(init_lambda,"Not initializing lambda");
       //casadi_assert_warning(init_z,"Not initializing z");
     }
     
     if(init_x){
       input(NLP_SOLVER_X0).getArray(x,n);
     }
   
     if (init_z) {
       // Get dual solution (simple bounds)
       vector<double>& lambda_x = input(NLP_SOLVER_LAM_X0).data();
       for(int i=0; i<lambda_x.size(); ++i){
         z_L[i] = max(0.,-lambda_x[i]);
         z_U[i] = max(0., lambda_x[i]);
       }
     }
   
     if (init_lambda){
       input(NLP_SOLVER_LAM_G0).getArray(lambda,m);
     }
   
     return true;
   } catch (exception& ex){
     cerr << "get_starting_point failed: " << ex.what() << endl;
     return false;
   }
 }
コード例 #15
0
ファイル: implicit_to_nlp.cpp プロジェクト: BrechtBa/casadi
  void QpToImplicit::init() {
    // Call the base class initializer
    ImplicitFunctionInternal::init();

    // Free variable in the NLP
    MX u = MX::sym("u", input(iin_).sparsity());

    // So that we can pass it on to createParent
    std::vector<MX> inputs;
    for (int i=0; i<nIn(); ++i) {
      if (i!=iin_) {
        stringstream ss;
        ss << "p" << i;
        inputs.push_back(MX::sym(ss.str(), input(i).sparsity()));
      }
    }
    MX p = veccat(inputs);

    // Dummy NLP objective
    MX nlp_f = 0;

    // NLP constraints
    std::vector< MX > args_call(nIn());
    args_call[iin_] = u;
    for (int i=0, i2=0; i<nIn(); ++i)
      if (i!=iin_) args_call[i] = inputs[i2++];
    MX nlp_g = f_(args_call).at(iout_);

    // We're going to use two-argument objective and constraints to allow the use of parameters
    MXFunction nlp("nlp", nlpIn("x", u, "p", p), nlpOut("f", nlp_f, "g", nlp_g));

    Dict options;
    if (hasSetOption(optionsname())) options = getOption(optionsname());
    // Create an NlpSolver instance
    solver_ = NlpSolver("nlpsolver", getOption(solvername()), nlp, options);
  }
コード例 #16
0
ファイル: sdqp_to_sdp.cpp プロジェクト: BrechtBa/casadi
  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);
  }
コード例 #17
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();
}
コード例 #18
0
  void WorhpInternal::passOptions() {
     
     for (int i=0;i<WorhpGetParamCount();++i) {
      WorhpType type = WorhpGetParamType(i+1);
      const char* name = WorhpGetParamName(i+1);
      if (strcmp(name,"Ares")==0) continue;

      switch(type) {
        case WORHP_BOOL_T:
          if (hasSetOption(name)) WorhpSetBoolParam(&worhp_p_, name, getOption(name));
          break;
        case WORHP_DOUBLE_T:
          if (hasSetOption(name)) WorhpSetDoubleParam(&worhp_p_, name, getOption(name));
          break;
        case WORHP_INT_T:
          if (hasSetOption(name)) WorhpSetIntParam(&worhp_p_, name, getOption(name));
          break;
        default:
          break;// do nothing
      }
    } 
  
    if (hasSetOption("qp_ipBarrier")) worhp_p_.qp.ipBarrier = getOption("qp_ipBarrier");
    if (hasSetOption("qp_ipComTol")) worhp_p_.qp.ipComTol = getOption("qp_ipComTol");
    if (hasSetOption("qp_ipFracBound")) worhp_p_.qp.ipFracBound = getOption("qp_ipFracBound");
    if (hasSetOption("qp_ipLsMethod")) worhp_p_.qp.ipLsMethod = getOptionEnumValue("qp_ipLsMethod");
    if (hasSetOption("qp_ipMinAlpha")) worhp_p_.qp.ipMinAlpha = getOption("qp_ipMinAlpha");
    if (hasSetOption("qp_ipTryRelax")) worhp_p_.qp.ipTryRelax = getOption("qp_ipTryRelax");
    if (hasSetOption("qp_ipRelaxDiv")) worhp_p_.qp.ipRelaxDiv = getOption("qp_ipRelaxDiv");
    if (hasSetOption("qp_ipRelaxMult")) worhp_p_.qp.ipRelaxMult = getOption("qp_ipRelaxMult");
    if (hasSetOption("qp_ipRelaxMax")) worhp_p_.qp.ipRelaxMax = getOption("qp_ipRelaxMax");
    if (hasSetOption("qp_ipRelaxMin")) worhp_p_.qp.ipRelaxMin = getOption("qp_ipRelaxMin");
    if (hasSetOption("qp_ipResTol")) worhp_p_.qp.ipResTol = getOption("qp_ipResTol");
    if (hasSetOption("qp_lsItMaxIter")) worhp_p_.qp.lsItMaxIter = getOption("qp_lsItMaxIter");
    if (hasSetOption("qp_lsItMethod")) worhp_p_.qp.lsItMethod = getOptionEnumValue("qp_lsItMethod");
    if (hasSetOption("qp_lsItPrecondMethod")) worhp_p_.qp.lsItPrecondMethod = getOptionEnumValue("qp_lsItPrecondMethod");
    if (hasSetOption("qp_lsRefineMaxIter")) worhp_p_.qp.lsRefineMaxIter = getOption("qp_lsRefineMaxIter");
    if (hasSetOption("qp_lsScale")) worhp_p_.qp.lsScale = getOption("qp_lsScale");
    if (hasSetOption("qp_lsTrySimple")) worhp_p_.qp.lsTrySimple = getOption("qp_lsTrySimple");
    if (hasSetOption("qp_lsTol")) worhp_p_.qp.lsTol = getOption("qp_lsTol");
    if (hasSetOption("qp_maxIter")) worhp_p_.qp.maxIter = getOption("qp_maxIter");
    if (hasSetOption("qp_method")) worhp_p_.qp.method = getOptionEnumValue("qp_method");
    if (hasSetOption("qp_nsnBeta")) worhp_p_.qp.nsnBeta = getOption("qp_nsnBeta");
    if (hasSetOption("qp_nsnGradStep")) worhp_p_.qp.nsnGradStep = getOption("qp_nsnGradStep");
    if (hasSetOption("qp_nsnKKT")) worhp_p_.qp.nsnKKT = getOption("qp_nsnKKT");
    if (hasSetOption("qp_nsnLsMethod")) worhp_p_.qp.nsnLsMethod = getOptionEnumValue("qp_nsnLsMethod");
    if (hasSetOption("qp_nsnMinAlpha")) worhp_p_.qp.nsnMinAlpha = getOption("qp_nsnMinAlpha");
    if (hasSetOption("qp_nsnSigma")) worhp_p_.qp.nsnSigma = getOption("qp_nsnSigma");
    if (hasSetOption("qp_printLevel")) worhp_p_.qp.printLevel = getOptionEnumValue("qp_printLevel");
    if (hasSetOption("qp_scaleIntern")) worhp_p_.qp.scaleIntern = getOption("qp_scaleIntern");
    if (hasSetOption("qp_strict")) worhp_p_.qp.strict = getOption("qp_strict");  

    // Mark the parameters as set
    worhp_p_.initialised = true;
  }
コード例 #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);
  }
コード例 #20
0
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();
}
コード例 #21
0
ファイル: sqpmethod.cpp プロジェクト: BrechtBa/casadi
  void Sqpmethod::init() {
    // Call the init method of the base class
    NlpSolverInternal::init();

    // Read options
    max_iter_ = getOption("max_iter");
    max_iter_ls_ = getOption("max_iter_ls");
    c1_ = getOption("c1");
    beta_ = getOption("beta");
    merit_memsize_ = getOption("merit_memory");
    lbfgs_memory_ = getOption("lbfgs_memory");
    tol_pr_ = getOption("tol_pr");
    tol_du_ = getOption("tol_du");
    regularize_ = getOption("regularize");
    exact_hessian_ = getOption("hessian_approximation")=="exact";
    min_step_size_ = getOption("min_step_size");

    // Get/generate required functions
    gradF();
    jacG();
    if (exact_hessian_) {
      hessLag();
    }

    // Allocate a QP solver
    Sparsity H_sparsity = exact_hessian_ ? hessLag().output().sparsity()
        : Sparsity::dense(nx_, nx_);
    H_sparsity = H_sparsity + Sparsity::diag(nx_);
    Sparsity A_sparsity = jacG().isNull() ? Sparsity(0, nx_)
        : jacG().output().sparsity();

    // QP solver options
    Dict qp_solver_options;
    if (hasSetOption("qp_solver_options")) {
      qp_solver_options = getOption("qp_solver_options");
    }

    // Allocate a QP solver
    qp_solver_ = QpSolver("qp_solver", getOption("qp_solver"),
                          make_map("h", H_sparsity, "a", A_sparsity),
                          qp_solver_options);

    // Lagrange multipliers of the NLP
    mu_.resize(ng_);
    mu_x_.resize(nx_);

    // Lagrange gradient in the next iterate
    gLag_.resize(nx_);
    gLag_old_.resize(nx_);

    // Current linearization point
    x_.resize(nx_);
    x_cand_.resize(nx_);
    x_old_.resize(nx_);

    // Constraint function value
    gk_.resize(ng_);
    gk_cand_.resize(ng_);

    // Hessian approximation
    Bk_ = DMatrix::zeros(H_sparsity);

    // Jacobian
    Jk_ = DMatrix::zeros(A_sparsity);

    // Bounds of the QP
    qp_LBA_.resize(ng_);
    qp_UBA_.resize(ng_);
    qp_LBX_.resize(nx_);
    qp_UBX_.resize(nx_);

    // QP solution
    dx_.resize(nx_);
    qp_DUAL_X_.resize(nx_);
    qp_DUAL_A_.resize(ng_);

    // Gradient of the objective
    gf_.resize(nx_);

    // Create Hessian update function
    if (!exact_hessian_) {
      // Create expressions corresponding to Bk, x, x_old, gLag and gLag_old
      SX Bk = SX::sym("Bk", H_sparsity);
      SX x = SX::sym("x", input(NLP_SOLVER_X0).sparsity());
      SX x_old = SX::sym("x", x.sparsity());
      SX gLag = SX::sym("gLag", x.sparsity());
      SX gLag_old = SX::sym("gLag_old", x.sparsity());

      SX sk = x - x_old;
      SX yk = gLag - gLag_old;
      SX qk = mul(Bk, sk);

      // Calculating theta
      SX skBksk = inner_prod(sk, qk);
      SX omega = if_else(inner_prod(yk, sk) < 0.2 * inner_prod(sk, qk),
                               0.8 * skBksk / (skBksk - inner_prod(sk, yk)),
                               1);
      yk = omega * yk + (1 - omega) * qk;
      SX theta = 1. / inner_prod(sk, yk);
      SX phi = 1. / inner_prod(qk, sk);
      SX Bk_new = Bk + theta * mul(yk, yk.T()) - phi * mul(qk, qk.T());

      // Inputs of the BFGS update function
      vector<SX> bfgs_in(BFGS_NUM_IN);
      bfgs_in[BFGS_BK] = Bk;
      bfgs_in[BFGS_X] = x;
      bfgs_in[BFGS_X_OLD] = x_old;
      bfgs_in[BFGS_GLAG] = gLag;
      bfgs_in[BFGS_GLAG_OLD] = gLag_old;
      bfgs_ = SXFunction("bfgs", bfgs_in, make_vector(Bk_new));

      // Initial Hessian approximation
      B_init_ = DMatrix::eye(nx_);
    }

    // Header
    if (static_cast<bool>(getOption("print_header"))) {
      userOut()
        << "-------------------------------------------" << endl
        << "This is casadi::SQPMethod." << endl;
      if (exact_hessian_) {
        userOut() << "Using exact Hessian" << endl;
      } else {
        userOut() << "Using limited memory BFGS Hessian approximation" << endl;
      }
      userOut()
        << endl
        << "Number of variables:                       " << setw(9) << nx_ << endl
        << "Number of constraints:                     " << setw(9) << ng_ << endl
        << "Number of nonzeros in constraint Jacobian: " << setw(9) << A_sparsity.nnz() << endl
        << "Number of nonzeros in Lagrangian Hessian:  " << setw(9) << H_sparsity.nnz() << endl
        << endl;
    }
  }
コード例 #22
0
void AcadoOCPInternal::evaluate(int nfdir, int nadir){ 
  // Initial constraint function
  if(!rfcn_.f_.isNull()){
    const Matrix<double>& lbr = input(ACADO_LBR);
    ACADO::Vector lb(lbr.size(),&lbr.front());
    const Matrix<double>& ubr = input(ACADO_UBR);
    ACADO::Vector ub(ubr.size(),&ubr.front());
    ocp_->subjectTo( ACADO::AT_START, lb <= (*rfcn_.fcn_)(*arg_) <= ub);
  }

  // Path constraint function
  if(!cfcn_.f_.isNull()){
    const Matrix<double>& lbc = input(ACADO_LBC);
    ACADO::Vector lb(lbc.size(),&lbc.front());
    const Matrix<double>& ubc = input(ACADO_UBC);
    ACADO::Vector ub(ubc.size(),&ubc.front());
    ocp_->subjectTo( lb <= (*cfcn_.fcn_)(*arg_) <=  ub );
  }

  // State bounds
  Matrix<double> &lbx = input(ACADO_LBX);
  Matrix<double> &ubx = input(ACADO_UBX);
  for(int i=0; i<nxd_; ++i)
    ocp_->subjectTo( lbx.at(i) <= xd_[i] <=  ubx.at(i) );
  for(int i=nxd_; i<nx_; ++i)
    ocp_->subjectTo( lbx.at(i) <= xa_[i-nxd_] <=  ubx.at(i) );

  // Pass bounds on state at initial time
  Matrix<double> &lbx0 = input(ACADO_LBX0);
  Matrix<double> &ubx0 = input(ACADO_UBX0);
  for(int i=0; i<nxd_; ++i)
    ocp_->subjectTo( ACADO::AT_START, lbx0.at(i) <= xd_[i] <=  ubx0.at(i) );
  for(int i=nxd_; i<nx_; ++i)
    ocp_->subjectTo( ACADO::AT_START, lbx0.at(i) <= xa_[i-nxd_] <=  ubx0.at(i) );

//     ocp_->subjectTo( AT_END  , xd_[1] ==  0.0 );
//     ocp_->subjectTo( AT_END  , xd_[2] ==  0.0 );

  // Control bounds
  Matrix<double> &lbu = input(ACADO_LBU);
  Matrix<double> &ubu = input(ACADO_UBU);
  for(int i=0; i<nu_; ++i)
    ocp_->subjectTo( lbu.at(i) <= u_[i] <= ubu.at(i) );

  // Parameter bounds
  Matrix<double> &lbp = input(ACADO_LBP);
  Matrix<double> &ubp = input(ACADO_UBP);
  for(int i=0; i<np_; ++i)
    ocp_->subjectTo( lbp.at(i) <= p_[i] <= ubp.at(i) );

  // Periodic boundary condition
  if(hasSetOption("periodic_bounds")){
    const vector<int>& periodic = getOption("periodic_bounds");
    if(periodic.size()!=nx_) throw CasadiException("wrong dimension for periodic_bounds");
    for(int i=0; i<nxd_; ++i)
      if(periodic[i])
        ocp_->subjectTo( 0.0, xd_[i], -xd_[i], 0.0);

    for(int i=nxd_; i<nx_; ++i)
      if(periodic[i])
        ocp_->subjectTo( 0.0, xa_[i-nxd_], -xa_[i-nxd_], 0.0);
  }
  
  algorithm_ = new ACADO::OptimizationAlgorithm(*ocp_);
  
  // set print level
  ACADO::PrintLevel printlevel;
  if(getOption("print_level")=="none")        printlevel = ACADO::NONE;
  else if(getOption("print_level")=="low")    printlevel = ACADO::LOW;
  else if(getOption("print_level")=="medium") printlevel = ACADO::MEDIUM;
  else if(getOption("print_level")=="high")   printlevel = ACADO::HIGH;
  else if(getOption("print_level")=="debug")  printlevel = ACADO::DEBUG;
  else throw CasadiException("Illegal print level. Allowed are \"none\", \"low\", \"medium\", \"high\", \"debug\"");
  algorithm_->set(ACADO::INTEGRATOR_PRINTLEVEL, printlevel );

  // Set integrator
  if(hasSetOption("integrator")){
    GenericType integ = getOption("integrator");
    ACADO::IntegratorType itype;
    if(integ=="rk4")           itype=ACADO::INT_RK4;
    else if(integ=="rk12")     itype=ACADO::INT_RK12;
    else if(integ=="rk23")     itype=ACADO::INT_RK23;
    else if(integ=="rk45")     itype=ACADO::INT_RK45;
    else if(integ=="rk78")     itype=ACADO::INT_RK78;
    else if(integ=="bdf")      itype=ACADO::INT_BDF;
    else if(integ=="discrete") itype=ACADO::INT_DISCRETE;
    else if(integ=="unknown")  itype=ACADO::INT_UNKNOWN;
    #ifdef ACADO_HAS_USERDEF_INTEGRATOR
    else if(integ=="casadi"){
      if(ACADO::Integrator::integrator_creator_ || ACADO::Integrator::integrator_user_data_)
        throw CasadiException("AcadoOCPInternal::AcadoOCPInternal: An instance already exists");
      
      if(integrators_.size() <= n_nodes_)
        throw CasadiException("AcadoOCPInternal::AcadoOCPInternal: Number of integrators does not match number of shooting nodes");
      
      ACADO::Integrator::integrator_creator_ = &AcadoIntegratorBackend::create;
      ACADO::Integrator::integrator_user_data_ = this;
      itype=ACADO::INT_UNKNOWN;
    }
    #endif
  else throw CasadiException("AcadoOCPInternal::evaluate: no such integrator: " + integ.toString());
    algorithm_->set(ACADO::INTEGRATOR_TYPE, itype);
  };
  
  // Set integrator tolerance
  if(hasSetOption("integrator_tolerance")) algorithm_->set( ACADO::INTEGRATOR_TOLERANCE, getOption("integrator_tolerance").toDouble());
  if(hasSetOption("absolute_tolerance")) algorithm_->set( ACADO::ABSOLUTE_TOLERANCE, getOption("absolute_tolerance").toDouble());
  if(hasSetOption("kkt_tolerance")) algorithm_->set( ACADO::KKT_TOLERANCE, getOption("kkt_tolerance").toDouble());
  if(hasSetOption("max_num_iterations")) algorithm_->set( ACADO::MAX_NUM_ITERATIONS, getOption("max_num_iterations").toInt() );
  if(hasSetOption("max_num_integrator_steps")) algorithm_->set( ACADO::MAX_NUM_INTEGRATOR_STEPS, getOption("max_num_integrator_steps").toInt() );
  if(hasSetOption("relaxation_parameter")) algorithm_->set( ACADO::RELAXATION_PARAMETER, getOption("relaxation_parameter").toDouble());
  
  if(hasSetOption("dynamic_sensitivity")){
    if(getOption("dynamic_sensitivity") == "forward_sensitivities")
      algorithm_->set( ACADO::DYNAMIC_SENSITIVITY,  ACADO::FORWARD_SENSITIVITY );
    else if(getOption("dynamic_sensitivity") == "backward_sensitivities")
      algorithm_->set( ACADO::DYNAMIC_SENSITIVITY,  ACADO::BACKWARD_SENSITIVITY );
    else 
      throw CasadiException("illegal dynamic_sensitivity");
  }

  if(hasSetOption("hessian_approximation")){
    int hess;
    GenericType op = getOption("hessian_approximation");
    if(op=="exact_hessian")                 hess = ACADO::EXACT_HESSIAN;
    else if(op == "constant_hessian")       hess = ACADO::CONSTANT_HESSIAN;
    else if(op == "full_bfgs_update")       hess = ACADO::FULL_BFGS_UPDATE;
    else if(op == "block_bfgs_update")      hess = ACADO::BLOCK_BFGS_UPDATE;
    else if(op == "gauss_newton")           hess = ACADO::GAUSS_NEWTON;
    else if(op == "gauss_newton_with_block_bfgs") hess = ACADO::GAUSS_NEWTON_WITH_BLOCK_BFGS;
    else throw CasadiException("illegal hessian approximation");
    
    algorithm_->set( ACADO::HESSIAN_APPROXIMATION,  hess);
  }

  // should the states be initialized by a forward integration?
  bool auto_init = getOption("auto_init").toInt();

  // Initialize differential states
  if(nxd_>0){
    // Initial guess
    Matrix<double> &x0 = input(ACADO_X_GUESS);
    
    // Assemble the variables grid
    ACADO::VariablesGrid xd(nxd_, n_nodes_+1);
    for(int i=0; i<n_nodes_+1; ++i){
      ACADO::Vector v(nxd_,&x0.at(i*nx_));
      xd.setVector(i,v);
    }
    
    // Pass to acado
    algorithm_->initializeDifferentialStates(xd,auto_init ? ACADO::BT_TRUE : ACADO::BT_FALSE);
  }
    
  // Initialize algebraic states
  if(nxa_>0){
    // Initial guess
    Matrix<double> &x0 = input(ACADO_X_GUESS);
    
    // Assemble the variables grid
    ACADO::VariablesGrid xa(nxa_, n_nodes_+1);
    for(int i=0; i<n_nodes_+1; ++i){
      ACADO::Vector v(nxa_,&x0.at(i*nx_+nxd_));
      xa.setVector(i,v);
    }
    
    // Pass to acado
    algorithm_->initializeAlgebraicStates(xa,auto_init ? ACADO::BT_TRUE : ACADO::BT_FALSE);
  }
    
  // Initialize controls
  if(nu_>0){
    // Initial guess
    Matrix<double> &u0 = input(ACADO_U_GUESS);
    
    // Assemble the variables grid
    ACADO::VariablesGrid u(nu_, n_nodes_+1);
    for(int i=0; i<n_nodes_+1; ++i){
      ACADO::Vector v(nu_,&u0.at(i*nu_));
      u.setVector(i,v);
    }
    
    // Pass to acado
    algorithm_->initializeControls(u);
  }
    
  // Initialize parameters
  if(np_>0){
    // Initial guess
    Matrix<double> &p0 = input(ACADO_P_GUESS);
    
    // Assemble the variables grid
    ACADO::VariablesGrid p(np_, n_nodes_+1);
    for(int i=0; i<n_nodes_+1; ++i){
      ACADO::Vector v(np_,&p0.front()); // NB!
      p.setVector(i,v);
    }
    
    // Pass to acado
    algorithm_->initializeParameters(p);
  }

  // Solve
  algorithm_->solve();

  // Get the optimal state trajectory
  if(nxd_>0){
    Matrix<double> &xopt = output(ACADO_X_OPT);
    ACADO::VariablesGrid xd;
    algorithm_->getDifferentialStates(xd);
    assert(xd.getNumPoints()==n_nodes_+1);
    for(int i=0; i<n_nodes_+1; ++i){
      // Copy to result
      ACADO::Vector v = xd.getVector(i);
      &xopt.at(i*nx_) << v;
    }
  }
  if(nxa_>0){
    Matrix<double> &xopt = output(ACADO_X_OPT);
    ACADO::VariablesGrid xa;
    algorithm_->getAlgebraicStates(xa);
    assert(xa.getNumPoints()==n_nodes_+1);
    for(int i=0; i<n_nodes_+1; ++i){
      // Copy to result
      ACADO::Vector v = xa.getVector(i);
      &xopt.at(i*nx_ + nxd_) << v;
    }
  }

  // Get the optimal control trajectory
  if(nu_>0){
    Matrix<double> &uopt = output(ACADO_U_OPT);
    ACADO::VariablesGrid u;
    algorithm_->getControls(u);
    assert(u.getNumPoints()==n_nodes_+1);
    for(int i=0; i<n_nodes_+1; ++i){
      // Copy to result
      ACADO::Vector v = u.getVector(i);
      &uopt.at(i*nu_) << v;
    }
  }

  // Get the optimal parameters
  if(np_>0){
    Matrix<double> &popt = output(ACADO_P_OPT);
    ACADO::Vector p;
    algorithm_->getParameters(p);
    &popt.front() << p;
  }
  
  // Get the optimal cost
  double cost = algorithm_->getObjectiveValue();
  output(ACADO_COST).set(cost);
}
コード例 #23
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();
}
コード例 #24
0
ファイル: sqp_internal.cpp プロジェクト: kozatt/casadi
void SQPInternal::init(){
  // Call the init method of the base class
  NLPSolverInternal::init();
    
  // Read options
  maxiter_ = getOption("maxiter");
  maxiter_ls_ = getOption("maxiter_ls");
  c1_ = getOption("c1");
  beta_ = getOption("beta");
  merit_memsize_ = getOption("merit_memory");
  lbfgs_memory_ = getOption("lbfgs_memory");
  tol_pr_ = getOption("tol_pr");
  tol_du_ = getOption("tol_du");
  regularize_ = getOption("regularize");
  if(getOption("hessian_approximation")=="exact")
    hess_mode_ = HESS_EXACT;
  else if(getOption("hessian_approximation")=="limited-memory")
    hess_mode_ = HESS_BFGS;
   
  if (hess_mode_== HESS_EXACT && H_.isNull()) {
    if (!getOption("generate_hessian")){
      casadi_error("SQPInternal::evaluate: you set option 'hessian_approximation' to 'exact', but no hessian was supplied. Try with option \"generate_hessian\".");
    }
  }
  
  // If the Hessian is generated, we use exact approximation by default
  if (bool(getOption("generate_hessian"))){
    setOption("hessian_approximation", "exact");
  }
  
  // Allocate a QP solver
  CRSSparsity H_sparsity = hess_mode_==HESS_EXACT ? H_.output().sparsity() : sp_dense(n_,n_);
  H_sparsity = H_sparsity + DMatrix::eye(n_).sparsity();
  CRSSparsity A_sparsity = J_.isNull() ? CRSSparsity(0,n_,false) : J_.output().sparsity();

  QPSolverCreator qp_solver_creator = getOption("qp_solver");
  qp_solver_ = qp_solver_creator(H_sparsity,A_sparsity);

  // Set options if provided
  if(hasSetOption("qp_solver_options")){
    Dictionary qp_solver_options = getOption("qp_solver_options");
    qp_solver_.setOption(qp_solver_options);
  }
  qp_solver_.init();
  
  // Lagrange multipliers of the NLP
  mu_.resize(m_);
  mu_x_.resize(n_);
  
  // Lagrange gradient in the next iterate
  gLag_.resize(n_);
  gLag_old_.resize(n_);

  // Current linearization point
  x_.resize(n_);
  x_cand_.resize(n_);
  x_old_.resize(n_);

  // Constraint function value
  gk_.resize(m_);
  gk_cand_.resize(m_);
  
  // Hessian approximation
  Bk_ = DMatrix(H_sparsity);
  
  // Jacobian
  Jk_ = DMatrix(A_sparsity);

  // Bounds of the QP
  qp_LBA_.resize(m_);
  qp_UBA_.resize(m_);
  qp_LBX_.resize(n_);
  qp_UBX_.resize(n_);

  // QP solution
  dx_.resize(n_);
  qp_DUAL_X_.resize(n_);
  qp_DUAL_A_.resize(m_);

  // Gradient of the objective
  gf_.resize(n_);

  // Create Hessian update function
  if(hess_mode_ == HESS_BFGS){
    // Create expressions corresponding to Bk, x, x_old, gLag and gLag_old
    SXMatrix Bk = ssym("Bk",H_sparsity);
    SXMatrix x = ssym("x",input(NLP_X_INIT).sparsity());
    SXMatrix x_old = ssym("x",x.sparsity());
    SXMatrix gLag = ssym("gLag",x.sparsity());
    SXMatrix gLag_old = ssym("gLag_old",x.sparsity());
    
    SXMatrix sk = x - x_old;
    SXMatrix yk = gLag - gLag_old;
    SXMatrix qk = mul(Bk, sk);
    
    // Calculating theta
    SXMatrix skBksk = inner_prod(sk, qk);
    SXMatrix omega = if_else(inner_prod(yk, sk) < 0.2 * inner_prod(sk, qk),
                             0.8 * skBksk / (skBksk - inner_prod(sk, yk)),
                             1);
    yk = omega * yk + (1 - omega) * qk;
    SXMatrix theta = 1. / inner_prod(sk, yk);
    SXMatrix phi = 1. / inner_prod(qk, sk);
    SXMatrix Bk_new = Bk + theta * mul(yk, trans(yk)) - phi * mul(qk, trans(qk));
    
    // Inputs of the BFGS update function
    vector<SXMatrix> bfgs_in(BFGS_NUM_IN);
    bfgs_in[BFGS_BK] = Bk;
    bfgs_in[BFGS_X] = x;
    bfgs_in[BFGS_X_OLD] = x_old;
    bfgs_in[BFGS_GLAG] = gLag;
    bfgs_in[BFGS_GLAG_OLD] = gLag_old;
    bfgs_ = SXFunction(bfgs_in,Bk_new);
    bfgs_.setOption("number_of_fwd_dir",0);
    bfgs_.setOption("number_of_adj_dir",0);
    bfgs_.init();
    
    // Initial Hessian approximation
    B_init_ = DMatrix::eye(n_);
  }
  
  // Header
  if(bool(getOption("print_header"))){
    cout << "-------------------------------------------" << endl;
    cout << "This is CasADi::SQPMethod." << endl;
    switch (hess_mode_) {
      case HESS_EXACT:
        cout << "Using exact Hessian" << endl;
        break;
      case HESS_BFGS:
        cout << "Using limited memory BFGS Hessian approximation" << endl;
        break;
    }
    cout << endl;
    cout << "Number of variables:                       " << setw(9) << n_ << endl;
    cout << "Number of constraints:                     " << setw(9) << m_ << endl;
    cout << "Number of nonzeros in constraint Jacobian: " << setw(9) << A_sparsity.size() << endl;
    cout << "Number of nonzeros in Lagrangian Hessian:  " << setw(9) << H_sparsity.size() << endl;
    cout << endl;
  }
}
コード例 #25
0
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;
}
コード例 #26
0
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();
}
コード例 #27
0
  void ImplicitFunctionInternal::init() {

    // Initialize the residual function
    f_.init(false);

    // Which input/output correspond to the root-finding problem?
    iin_ = getOption("implicit_input");
    iout_ = getOption("implicit_output");

    // Get the number of equations and check consistency
    casadi_assert_message(iin_>=0 && iin_<f_.getNumInputs() && f_.getNumInputs()>0,
                          "Implicit input not in range");
    casadi_assert_message(iout_>=0 && iout_<f_.getNumOutputs() && f_.getNumOutputs()>0,
                          "Implicit output not in range");
    casadi_assert_message(f_.output(iout_).isDense() && f_.output(iout_).isVector(),
                          "Residual must be a dense vector");
    casadi_assert_message(f_.input(iin_).isDense() && f_.input(iin_).isVector(),
                          "Unknown must be a dense vector");
    n_ = f_.output(iout_).size();
    casadi_assert_message(n_ == f_.input(iin_).size(),
                          "Dimension mismatch. Input size is "
                          << f_.input(iin_).size()
                          << ", while output size is "
                          << f_.output(iout_).size());

    // Allocate inputs
    setNumInputs(f_.getNumInputs());
    for (int i=0; i<getNumInputs(); ++i) {
      input(i) = f_.input(i);
    }

    // Allocate output
    setNumOutputs(f_.getNumOutputs());
    for (int i=0; i<getNumOutputs(); ++i) {
      output(i) = f_.output(i);
    }

    // Same input and output schemes
    setInputScheme(f_.getInputScheme());
    setOutputScheme(f_.getOutputScheme());

    // Call the base class initializer
    FunctionInternal::init();

    // Generate Jacobian if not provided
    if (jac_.isNull()) jac_ = f_.jacobian(iin_, iout_);
    jac_.init(false);

    // Check for structural singularity in the Jacobian
    casadi_assert_message(
      !jac_.output().sparsity().isSingular(),
      "ImplicitFunctionInternal::init: singularity - the jacobian is structurally rank-deficient. "
      "sprank(J)=" << sprank(jac_.output()) << " (instead of "<< jac_.output().size1() << ")");

    // Get the linear solver creator function
    if (linsol_.isNull()) {
      if (hasSetOption("linear_solver")) {
        std::string linear_solver_name = getOption("linear_solver");

        // Allocate an NLP solver
        linsol_ = LinearSolver(linear_solver_name, jac_.output().sparsity(), 1);

        // Pass options
        if (hasSetOption("linear_solver_options")) {
          const Dictionary& linear_solver_options = getOption("linear_solver_options");
          linsol_.setOption(linear_solver_options);
        }

        // Initialize
        linsol_.init();
      }
    } else {
      // Initialize the linear solver, if provided
      linsol_.init(false);
      casadi_assert(linsol_.input().sparsity()==jac_.output().sparsity());
    }

    // No factorization yet;
    fact_up_to_date_ = false;

    // Constraints
    if (hasSetOption("constraints")) u_c_ = getOption("constraints");

    casadi_assert_message(u_c_.size()==n_ || u_c_.empty(),
                          "Constraint vector if supplied, must be of length n, but got "
                          << u_c_.size() << " and n = " << n_);
  }
コード例 #28
0
  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();

  }
コード例 #29
0
  void QpoasesInterface::init() {
    QpSolverInternal::init();

    // Read options
    if (hasSetOption("nWSR")) {
      max_nWSR_ = getOption("nWSR");
      casadi_assert(max_nWSR_>=0);
    } else {
      max_nWSR_ = 5 *(n_ + nc_);
    }

    if (hasSetOption("CPUtime")) {
      max_cputime_ = getOption("CPUtime");
      casadi_assert(max_cputime_>0);
    } else {
      max_cputime_ = -1;
    }

    // Create data for H if not dense
    if (!input(QP_SOLVER_H).sparsity().isDense()) h_data_.resize(n_*n_);

    // Create data for A
    a_data_.resize(n_*nc_);

    // Dual solution vector
    dual_.resize(n_+nc_);

    // Create qpOASES instance
    if (qp_) delete qp_;
    if (ALLOW_QPROBLEMB && nc_==0) {
      qp_ = new qpOASES::QProblemB(n_);
    } else {
      qp_ = new qpOASES::SQProblem(n_, nc_);
    }
    called_once_ = false;

#ifdef ALLOW_ALL_OPTIONS
    qpOASES::Options ops;
    ops.setToDefault();
    ops.printLevel = string_to_PrintLevel(getOption("printLevel"));
    ops.enableRamping = bool_to_BooleanType(getOption("enableRamping"));
    ops.enableFarBounds = bool_to_BooleanType(getOption("enableFarBounds"));
    ops.enableFlippingBounds = bool_to_BooleanType(getOption("enableFlippingBounds"));
    ops.enableRegularisation = bool_to_BooleanType(getOption("enableRegularisation"));
    ops.enableFullLITests = bool_to_BooleanType(getOption("enableFullLITests"));
    ops.enableNZCTests = bool_to_BooleanType(getOption("enableNZCTests"));
    ops.enableDriftCorrection = static_cast<int>(getOption("enableDriftCorrection"));
    ops.enableCholeskyRefactorisation =
      static_cast<int>(getOption("enableCholeskyRefactorisation"));
    ops.enableEqualities = bool_to_BooleanType(getOption("enableEqualities"));
    ops.terminationTolerance = getOption("terminationTolerance");
    ops.boundTolerance = getOption("boundTolerance");
    ops.boundRelaxation = getOption("boundRelaxation");
    ops.epsNum = getOption("epsNum");
    ops.epsDen = getOption("epsDen");
    ops.maxPrimalJump = getOption("maxPrimalJump");
    ops.maxDualJump = getOption("maxDualJump");
    ops.initialRamping = getOption("initialRamping");
    ops.finalRamping = getOption("finalRamping");
    ops.initialFarBounds = getOption("initialFarBounds");
    ops.growFarBounds = getOption("growFarBounds");
    ops.initialStatusBounds = string_to_SubjectToStatus(getOption("initialStatusBounds"));
    ops.epsFlipping = getOption("epsFlipping");
    ops.numRegularisationSteps = static_cast<int>(getOption("numRegularisationSteps"));
    ops.epsRegularisation = getOption("epsRegularisation");
    ops.numRefinementSteps = static_cast<int>(getOption("numRefinementSteps"));
    ops.epsIterRef = getOption("epsIterRef");
    ops.epsLITests = getOption("epsLITests");
    ops.epsNZCTests = getOption("epsNZCTests");

    // Pass to qpOASES
    qp_->setOptions(ops);
#else // ALLOW_ALL_OPTIONS
    qp_->setPrintLevel(string_to_PrintLevel(getOption("printLevel")));
#endif // ALLOW_ALL_OPTIONS
  }
コード例 #30
0
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();
}