C++ casadi_assert函数代码示例

  Sparsity CSparseCholeskyInterface::linsol_cholesky_sparsity(void* mem, bool tr) const {
    auto m = static_cast<CsparseCholMemory*>(mem);

    casadi_assert(m->S);
    int n = m->A.n;
    int nzmax = m->S->cp[n];
    std::vector< int > row(n+1);
    std::copy(m->S->cp, m->S->cp+n+1, row.begin());
    std::vector< int > colind(nzmax);
    int *Li = &colind.front();
    int *Lp = &row.front();
    const cs* C;
    C = m->S->pinv ? cs_symperm(&m->A, m->S->pinv, 1) : &m->A;
    std::vector< int > temp(2*n);
    int *c = &temp.front();
    int *s = c+n;
    for (int k = 0 ; k < n ; k++) c[k] = m->S->cp[k] ;
    for (int k = 0 ; k < n ; k++) {       /* compute L(k, :) for L*L' = C */
      int top = cs_ereach(C, k, m->S->parent, s, c) ;
      for ( ; top < n ; top++) {  /* solve L(0:k-1, 0:k-1) * x = C(:, k) */
          int i = s[top] ;               /* s[top..n-1] is pattern of L(k, :) */
          int p = c[i]++ ;
          Li[p] = k ;                /* store L(k, i) in row i */
      }
      int p = c[k]++ ;
      Li[p] = k ;
    }
    Lp[n] = m->S->cp[n] ;
    Sparsity ret(n, n, row, colind); // BUG?

    return tr ? ret.T() : ret;

  }

  bool IpoptInternal::eval_grad_f(int n, const double* x, bool new_x, double* grad_f)
  {
    try {
      log("eval_grad_f started");
      double time1 = clock();
      casadi_assert(n == nx_);
    
      // Pass the argument to the function
      gradF_.setInput(x,NL_X);
      gradF_.setInput(input(NLP_SOLVER_P),NL_P);
      
      // Evaluate, adjoint mode
      gradF_.evaluate();
      
      // Get the result
      gradF_.output().getArray(grad_f,n,DENSE);
      
      // Printing
      if(monitored("eval_grad_f")){
        cout << "x = " << gradF_.input(NL_X) << endl;
        cout << "grad_f = " << gradF_.output() << endl;
      }

      if (regularity_check_ && !isRegular(gradF_.output().data())) casadi_error("IpoptInternal::grad_f: NaN or Inf detected.");
    
      double time2 = clock();
      t_eval_grad_f_ += double(time2-time1)/CLOCKS_PER_SEC;
      n_eval_grad_f_ += 1;
      log("eval_grad_f ok");
      return true;
    } catch (exception& ex){
      cerr << "eval_grad_f failed: " << ex.what() << endl;
      return false;
    }
  }

  void Options::check(const Dict& opts) const {
    // Make sure all options exist and have the correct type
    for (auto&& op : opts) {
      const Options::Entry* entry = find(op.first);

      // Informative error message if option does not exist
      if (entry==nullptr) {
        stringstream ss;
        ss << "Unknown option: " << op.first << endl;
        ss << endl;
        ss << "Did you mean one of the following?" << endl;
        for (auto&& s : suggestions(op.first)) {
          print_one(s, ss);
        }
        ss << "Use print_options() to get a full list of options." << endl;
        casadi_error(ss.str());
      }

      // Check type
      casadi_assert(op.second.can_cast_to(entry->type),
                            "Illegal type for " + op.first + ": " +
                            op.second.get_description() +
                            " cannot be cast to " +
                            GenericType::get_type_description(entry->type) + ".");

    }
  }

 bool IpoptInternal::get_bounds_info(int n, double* x_l, double* x_u,
                                     int m, double* g_l, double* g_u)
 {
   try {
     casadi_assert(n == nx_);
     casadi_assert(m == ng_);
     input(NLP_SOLVER_LBX).getArray(x_l,n);
     input(NLP_SOLVER_UBX).getArray(x_u,n);
     input(NLP_SOLVER_LBG).getArray(g_l,m);
     input(NLP_SOLVER_UBG).getArray(g_u,m);
     return true;
   } catch (exception& ex){
     cerr << "get_bounds_info failed: " << ex.what() << endl;
     return false;
   }
 }

void DirectMultipleShootingInternal::setOptimalSolution(const vector<double> &V_opt){
  // OCP solution
  Matrix<double> &p_opt = output(OCP_P_OPT);
  Matrix<double> &x_opt = output(OCP_X_OPT);
  Matrix<double> &u_opt = output(OCP_U_OPT);
  
  // Running index
  int el=0;

  // Pass optimized state
  for(int i=0; i<np_; ++i){
    p_opt(i) = V_opt[el++];
  }
    
  for(int k=0; k<nk_; ++k){
    
    // Pass optimized state
    for(int i=0; i<nx_; ++i){
      x_opt(i,k) = V_opt[el++];
    }
    
    // Pass optimized control
    for(int i=0; i<nu_; ++i){
      u_opt(i,k) = V_opt[el++];
    }
  }

  // Pass optimized terminal state
  for(int i=0; i<nx_; ++i){
    x_opt(i,nk_) = V_opt[el++];
  }
  casadi_assert(el==V_opt.size());
}

 CRSSparsity CSparseCholeskyInternal::getFactorizationSparsity() const {
   casadi_assert(S_);
   int n = AT_.n;
   int nzmax = S_->cp[n];
   std::vector< int > col(n+1);
   std::copy(S_->cp,S_->cp+n+1,col.begin());
   std::vector< int > rowind(nzmax);
   int *Li = &rowind.front();
   int *Lp = &col.front();
   const cs* C;
   C = S_->pinv ? cs_symperm (&AT_, S_->pinv, 1) : &AT_;
   std::vector< int > temp(2*n);
   int *c = & temp.front();
   int *s = c+n;
   for (int k = 0 ; k < n ; k++) c [k] = S_->cp [k] ;
   for (int k = 0 ; k < n ; k++) {       /* compute L(k,:) for L*L' = C */
     int top = cs_ereach (C, k, S_->parent, s, c) ;
     for ( ; top < n ; top++)    /* solve L(0:k-1,0:k-1) * x = C(:,k) */
       {
         int i = s [top] ;               /* s [top..n-1] is pattern of L(k,:) */
         int p = c [i]++ ;
         Li [p] = k ;                /* store L(k,i) in column i */
       }
     int p = c [k]++ ;
     Li [p] = k ;    
   }
   Lp [n] = S_->cp [n] ; 
   return trans(CRSSparsity(n, n, rowind, col));
 
 }

bool bad_test4(){
  // This will fail
  casadi_assert(bad_test3());
  
  // Returns true, but the code won't reach this place
  return true;
}

void DirectMultipleShootingInternal::getGuess(vector<double>& V_init) const{
  // OCP solution guess
  const Matrix<double> &p_init = input(OCP_P_INIT);
  const Matrix<double> &x_init = input(OCP_X_INIT);
  const Matrix<double> &u_init = input(OCP_U_INIT);
  
  // Running index
  int el=0;
  
  // Pass guess for parameters
  for(int i=0; i<np_; ++i){
    V_init[el++] = p_init.elem(i);
  }
  
  for(int k=0; k<nk_; ++k){
    // Pass guess for state
    for(int i=0; i<nx_; ++i){
      V_init[el++] = x_init.elem(i,k);
    }
    
    // Pass guess for control
    for(int i=0; i<nu_; ++i){
      V_init[el++] = u_init.elem(i,k);
    }
  }
  
  // Pass guess for final state
  for(int i=0; i<nx_; ++i){
    V_init[el++] = x_init.elem(i,nk_);
  }
  
  casadi_assert(el==V_init.size());
}

  void FixedStepIntegrator::integrateB(double t_out) {
    // Get discrete time sought
    int k_out = std::floor((t_out-t0_)/h_);
    k_out = std::max(k_out, 0); //  make sure that rounding errors does not result in k_out>nk_
    casadi_assert(k_out<=nk_);

    // Explicit discrete time dynamics
    Function& G = getExplicitB();

    // Take time steps until end time has been reached
    while (k_>k_out) {
      // Advance time
      k_--;
      t_ = t0_ + k_*h_;

      // Take step
      G.input(RDAE_T).set(t_);
      G.input(RDAE_X).setNZ(x_tape_.at(k_));
      G.input(RDAE_Z).setNZ(Z_tape_.at(k_));
      G.input(RDAE_P).set(input(INTEGRATOR_P));
      G.input(RDAE_RX).set(output(INTEGRATOR_RXF));
      G.input(RDAE_RZ).set(RZ_);
      G.input(RDAE_RP).set(input(INTEGRATOR_RP));
      G.evaluate();
      G.output(RDAE_ODE).get(output(INTEGRATOR_RXF));
      G.output(RDAE_ALG).get(RZ_);
      copy(RZ_.end()-nrz_, RZ_.end(), output(INTEGRATOR_RZF).begin());
      transform(G.output(RDAE_QUAD).begin(),
                G.output(RDAE_QUAD).end(),
                output(INTEGRATOR_RQF).begin(),
                output(INTEGRATOR_RQF).begin(),
                std::plus<double>());
    }
  }

void DirectSingleShootingInternal::setOptimalSolution(const vector<double> &V_opt){
  // OCP solution
  Matrix<double> &p_opt = output(OCP_P_OPT);
  Matrix<double> &x_opt = output(OCP_X_OPT);
  Matrix<double> &u_opt = output(OCP_U_OPT);
  
  // Running index
  int el=0;

  // Pass optimized parameters
  for(int i=0; i<np_; ++i){
    p_opt(i) = V_opt[el++];
  }
    
  // Pass optimized initial state
  for(int i=0; i<nx_; ++i){
    x_opt(i,0) = V_opt[el++];
  }
  
  // Pass optimized control
  for(int k=0; k<nk_; ++k){
    for(int i=0; i<nu_; ++i){
      u_opt(i,k) = V_opt[el++];
    }
  }
  casadi_assert(el==V_opt.size());

  // Evaluate the constraint function to get the rest of the state trajectory
  G_.setInput(V_opt);
  G_.evaluate();
  const vector<double>& g_opt = G_.output().data();

  // Loop over the constraints
  el = 0;
  for(int k=0; k<nk_; ++k){

    // Get the state trajectory
    for(int i=0; i<nx_; ++i){
      x_opt(i,k+1) = g_opt[el++];
    }
    
    // Skip the path constraints (for now)
    el += nh_;
  }
  casadi_assert(el==g_opt.size());
}

 std::string CodeGenerator::workelement(int n) {
   casadi_assert(n>=0);
   if (n==0) {
     return "*w";
   } else {
     return "w[+" + numToString(n) + "]";
   }
 }

  void SXFunctionInternal::spEvaluateOpenCL(bool fwd) {
    // OpenCL return flag
    cl_int ret;

    // Select a kernel
    cl_kernel kernel = fwd ? sp_fwd_kernel_ : sp_adj_kernel_;

    // Set OpenCL Kernel Parameters
    int kernel_arg = 0;

    // Pass inputs
    for (int i=0; i<nIn(); ++i) {
      ret = clSetKernelArg(kernel, kernel_arg++,
                           sizeof(cl_mem), static_cast<void *>(&sp_input_memobj_[i]));
      casadi_assert(ret == CL_SUCCESS);
    }

    // Pass outputs
    for (int i=0; i<nOut(); ++i) {
      ret = clSetKernelArg(kernel, kernel_arg++,
                           sizeof(cl_mem), static_cast<void *>(&sp_output_memobj_[i]));
      casadi_assert(ret == CL_SUCCESS);
    }

    // Execute OpenCL Kernel
    executeKernel(kernel);

    // Get inputs
    for (int i=0; i<sp_input_memobj_.size(); ++i) {
      ret = clEnqueueReadBuffer(sparsity_propagation_kernel_.command_queue,
                                sp_input_memobj_[i], CL_TRUE, 0,
                                inputNoCheck(i).size() * sizeof(cl_ulong),
                                reinterpret_cast<void*>(inputNoCheck(i).ptr()), 0, NULL, NULL);
      casadi_assert(ret == CL_SUCCESS);
    }

    // Get outputs
    for (int i=0; i<sp_output_memobj_.size(); ++i) {
      ret = clEnqueueReadBuffer(sparsity_propagation_kernel_.command_queue,
                                sp_output_memobj_[i], CL_TRUE, 0,
                                outputNoCheck(i).size() * sizeof(cl_ulong),
                                reinterpret_cast<void*>(outputNoCheck(i).ptr()), 0, NULL, NULL);
      casadi_assert(ret == CL_SUCCESS);
    }
  }

 MX SymbolicMX::join_primitives(std::vector<MX>::const_iterator& it) const {
   MX ret = *it++;
   if (ret.size()==size()) {
     return ret;
   } else {
     casadi_assert(ret.is_empty(true));
     return MX(size());
   }
 }

 IOSchemeCustomInternal::IOSchemeCustomInternal(const std::vector<std::string> &entries,
                                                const std::vector<std::string> &descriptions) :
     entries_(entries), descriptions_(descriptions)  {
   for (int i=0;i<entries.size();++i) {
     entrymap_[entries[i]] = i;
   }
   if (descriptions_.empty())  descriptions_.resize(entries.size());
   casadi_assert(descriptions_.size()==entries.size());
 }

  SwitchInternal::SwitchInternal(const std::vector<Function>& f, const Function& f_def)
    : f_(f), f_def_(f_def) {

    // Consitency check
    casadi_assert(!f_.empty());

    // Give a name
    setOption("name", "unnamed_switch");
  }